// For testing CNC milling spline //
#include <LCD_I2C.h>  // LCD library 
#include  <Wire.h>              //I2C library
#include <Keypad.h>             //Keypad Library
#include <EEPROM.h>             // EEPROM Library

LCD_I2C lcd(0x27, 16, 4); //LCD communication 20 x 4
char lcdbufferA [12]; // LCD buffer for A Axis
// for selector switch
const int SW1 = 30; // Pin 30-37 Selector switch
const int SW2 = 31; 
const int SW3 = 32; 
const int SW4 = 33; 
const int SW5 = 34; 
const int SW6 = 35; 
const int SW7 = 36; 
const int SW8 = 37;
int SelectSW = 0; // Selection Switch current
int OSelectSW = 0; // Selection switch old ref
int homerefset = 0; // for home ref set after start
int homeref2 = 0; // for selection switch ref memory
int emrgSW = 2; // emergency switch
int emrgTestS = 0; // emergency test status after start
int emrgTestS2 = 0; // emergency state

// for jog function
const int JSWXp = 9; // Jog X+
const int JSWXm = 10; // Jog X-
const int JSWZp = 7; // Jog Z+
const int JSWZm = 8; // Jog Z+
const int JSWAp = 5; // Jog Z+
const int JSWAm = 6; // Jog Z+
unsigned long previousMillis = 0; // time based movement p
unsigned long currentMillis = 0; // time based movement C
unsigned long interval = 2; // (milliseconds) half-interval for stepping A
unsigned long interval2 = 4; // (milliseconds) half-interval  home ref jog XZA
bool stepState = LOW;     // to start a stepper pulse

// for rotay encoder switch
const int encC = 11; // encoder clock
const int encD = 12; // encoder data, switch  not used as of now
bool encL = HIGH; // encode clock last state
bool encN = HIGH; // encode clock new state
long encP = 0; // for encoder position
long encPP = 0; // for encode previous postion
int encAx = 0; // for active axis ref
float encstp1X=0; // encoder steps 1X handle
float encstp10X=0; // encoder steps 10X handle
float encstp100x=0; // encode steps 100x handle

// for manual home ref and limit
const int lmtX1 = 46; // Pin 46-49 for home ref sensor
const int lmtX2 = 47; // X Axis second sensor optional as of now
const int lmtZ1 = 48; // Z Axis home sensor
const int lmtZ2 = 49; // Z Axis second sensor optional as of now

// for input key board
const int ROW_NUM = 4; //four rows keypad
const int COLUMN_NUM = 4; //four columns keypad
char keys[ROW_NUM][COLUMN_NUM] = {
  {'1','2','3', 'A'},
  {'4','5','6', 'B'},
  {'7','8','9', 'C'},
  {'.','0','#', 'D'}
};                          //Keypad buttons
byte pin_rows[ROW_NUM] = {38, 39, 40, 41}; //connect to the row pinouts of the keypad
byte pin_column[COLUMN_NUM] = {42, 43, 44, 45}; //connect to the column pinouts of the keypad
Keypad Akeypad = Keypad( makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM );
char customKey; // Value pressed on Keypad
String Keyn1; // Value pressed on Keypad n1
String Keyn2; // Value pressed on Keypad n2

// For Edit mode and Auto mode

// postions in mm
float cXm= 0; // for use in loop to move X
float cZm= 0; // for use in loop to move Z
float cAm=0; // for use in loop to move A
int Anoss = 1; // Index postion of rotation for logic in loop
int autostp=0; // for use in loop for step of loop in auto mode

// program vaiables as per JOB type
float cX0= 10; // for back from 0 ref for next cut + only
float cZ0= 7;  // for back from 0 ref for next cut + only
float cX1; // for ref position 0 of job to set in EEPROM
float cZ1;  // for ref position 0 of job to set in EEPROM 
float cA1 =0; // for ref position 0 of job A Axis
float cX2; // for depth of cut to set in EEPROM + only
float cZ2; // for Length of cut to set in EEPROM + only
int Anos; // No of splines to set in EEPROM
float fdrX= 2; // feed rate depth (to be used)
float fdrZ = 5; // feed rate side wise (to be used)
String strinp ; // for serial read
char sa; // char var part 1
String sb; // string var part 2
float sv; // string value

// For Auto Mode
const int cyclestart = 4; // Pin 4 cycle start in auto mode
const int cyclestop = 3; // Pin 4 cycle start in auto mode
int cyclestatus = 0; // cycle status
const int spdlstart = 53; // Spindle start output
const int clntstart = 52; // Coolant motor start output
const int spdlsw = 15; // spindle motor swtich for jog mode
const int jobref = 14; // for manual job ref switch
int debounce = 5000; // for delay at input for noise

int currentCommandIndex = 0; // not in use as of now

// For Stepper Controller //
#include <AccelStepper.h>
/* Stepper 1/X 
steps per rev 400 (full step setting), 5 mm per rev ball screw
mmstepsZ=400/5 = 80*/
const float mmstepsX = 80; // for drive setting 400  ...x2
/* Stepper 2/Z 
steps per rev 400 (full step setting), 1:195 Cycloid,
50 teeth drive gear, tooth pitch 10 mm,
1 rotation of stepper @400 steps, and 1/195*50*10 mm
mmstepsX= 400*195/50/10 = 156 */
const float mmstepsZ = 156; // for drive setting 400  ...x2
/* Stepper 3/A 
steps per rev 400 (full step setting), gear ratio 60
rstepsA=400*60*2= 48000*/
const float rstepsA = 48000; // for drive setting 800 ...x2 
AccelStepper mystepperX(AccelStepper::DRIVER, 22, 23);
AccelStepper mystepperZ(AccelStepper::DRIVER, 24, 25);
AccelStepper mystepperA(AccelStepper::DRIVER, 26, 27);
  int dirX = -1; // for reverse direction of stepper -1,1
  int dirZ= -1; // for reverse direction of stepper -1,1
  bool bdirX = 0; // for reverse direction of stepper 0, 1
  bool bdirZ = 0; // for reverse direction of stepper 0, 1


// For stepper position and movement

float jbrefX ; // job to home ref = cX1
float jbrefZ ; // job to home ref = cZ1

int hrX = 0; // for machine home ref after power on logic
int hrZ = 0; // for machine home ref after power on logic
float mmX; // Actual position of X w.r.t Home ref
float mmZ; // Actual position of Z w.r.t to Home ref
float relX = 0; // Relative position of X //
float relZ = 0; // Relative position of Z //
float mmXmv; // Max move mm X //
float mmZmv; // Max move mm Z //
float degA ; // Actual position of A //
float degAmv; // Max move deg A //
long stX; // Steps Number X //
long stZ; // Steps Number Z //
long stA; // Steps Number A //
float sts=100; // Step to move in each loop e.g code not in  use//
int Ljsw = 0; // last position of jog switch selection

void setup() {
  Serial.begin(115200);
  pinMode(SW1, INPUT_PULLUP); // Pin type 30-37 Selector switch 
  pinMode(SW2, INPUT_PULLUP);
  pinMode(SW3, INPUT_PULLUP);
  pinMode(SW4, INPUT_PULLUP);
  pinMode(SW5, INPUT_PULLUP);
  pinMode(SW6, INPUT_PULLUP);
  pinMode(SW7, INPUT_PULLUP);
  pinMode(SW8, INPUT_PULLUP);
  pinMode(JSWXp, INPUT_PULLUP); // Jog Switch pin type
  pinMode(JSWXm, INPUT_PULLUP);
  pinMode(JSWZp, INPUT_PULLUP);
  pinMode(JSWZm, INPUT_PULLUP);
  pinMode(JSWAp, INPUT_PULLUP);
  pinMode(JSWAm, INPUT_PULLUP); 
  pinMode(cyclestart, INPUT_PULLUP);  // for cycle start in automode

  pinMode(lmtX1, INPUT); // Pin type 30-37 limit switches
  pinMode(lmtX2, INPUT); // May have to redfine as per sensor
  pinMode(lmtZ1, INPUT); // for interal pull up register 
  pinMode(lmtZ2, INPUT);  // with actual sensors
  pinMode(encC, INPUT); // for encoder clock
  pinMode(encD,INPUT); // for encoder direction

  pinMode(emrgSW, INPUT_PULLUP); // emergency switch as ref
  pinMode(jobref, INPUT_PULLUP); // for manual job ref

  pinMode(spdlstart, OUTPUT); // Spindle start output
  digitalWrite(spdlstart, HIGH); // as per 1.9.24
  pinMode(clntstart, OUTPUT); // Coolant motor start output
  digitalWrite(clntstart, HIGH); // as per 1.9.24
  pinMode(spdlsw, INPUT_PULLUP); // spindle start switch
    //initialize lcd screen //
  lcd.begin();
  // turn on the backlight //
  lcd.backlight();
  // stepper initial parameters
  mystepperX.setMaxSpeed(1000);
  mystepperZ.setMaxSpeed(1000);
  mystepperA.setMaxSpeed(2000);
  mystepperX.setAcceleration(1000);
  mystepperZ.setAcceleration(1000);
  mystepperA.setAcceleration(2000);

// For Max movement soft limit for future//
  mmXmv = 6.2;
  mmZmv= 300.4;
  degAmv = 36.2;

  // For Display at Start up
  lcd.setCursor(5, 0);
  lcd.print("AUTOMATIC"); 
  lcd.setCursor(2, 1);
  lcd.print("SPLINE  CUTTING"); 
  lcd.setCursor(0, 2);
  lcd.print("  "); 
  lcd.setCursor(1, 3);
  lcd.print("ELLENABAD- 125102"); 
  delay(5000);
  lcd.clear();

  // not to use this in actual board for EEPROM
      jbrefX = -10; //for job ref from home
      jbrefZ= -10, // for job ref from home
      cX2 =2; // for depth of cut to set in EEPROM + only
      cZ2 =5; // for Length of cut to set in EEPROM + only
      Anos=3; // No of splines to set in EEPROM
      EEPROM.put(10, jbrefX); //for simulator only
      EEPROM.put(20, jbrefZ); // for simulator only
      EEPROM.put(30, cX2); // for simulator only
      EEPROM.put(40, cZ2); // for simulator only 
      EEPROM.put(50, Anos); // for simulator only 
  // to comment above while uploading sketch    

  // to load the data saved in EEPROM
  EEPROM.get(10,jbrefX); // read last job ref position
  EEPROM.get(20,jbrefZ); // read last job ref position
  EEPROM.get(30,cX2); // read last job ref position
  EEPROM.get(40,cZ2); // read last job ref position  
  EEPROM.get(50,Anos); // read last job ref position
  cX1 = jbrefX; // job ref from home - duplicate var
  cZ1 = jbrefZ; // job ref from home - duplicate var

  // For Emergency Switch interrupt
  // attachInterrupt(digitalPinToInterrupt(2), EInterrupt, FALLING);
  // intrupt not in use

}

// intrupt not is use
void EInterrupt() {

      if (emrgTestS ==1){
      delayMicroseconds(debounce);
        if (digitalRead((emrgSW) == LOW)){
          emrgTestS = 2; // Change the status as emergency
        }
      }

}

void loop() {

  if (emrgTestS == 2){ // If emergency button intrupt
            if (digitalRead((emrgSW) == LOW)){
            delayMicroseconds(debounce);
            if (digitalRead((emrgSW) == LOW)){
              if (emrgTestS2==0){
                clearRow(0); // Clear lcd Row 1
                clearRow(1); // Clear lcd Row 2
                clearRow(2); // Clear lcd Row 2
                lcd.setCursor(0, 0);
                lcd.print("EMG. Switch Pressed");
                emrgTestS2 = 1;   // Emergency state yes
              }
            }
            }

            if(digitalRead((emrgSW)) == HIGH){
              clearRow(0); // Clear lcd Row 1
              clearRow(1); // Clear lcd Row 2
              clearRow(2); // Clear lcd Row 2
              lcd.setCursor(0, 1);
              lcd.print("Emergency Clear");
              emrgTestS2 = 0; // Emergency state clear
              delay (1000);
              emrgTestS = 1;  // Machine in operation condition
              clearRow(0); // Clear lcd Row 0
              clearRow(1); // Clear lcd Row 1
              clearRow(2); // Clear lcd Row 2
            }
    goto skipallref; // for end position
  }

  if (SelectSW == 0) { // when machine is started
      if (emrgTestS !=1){
        goto emrgTest;  // go to emergency test 
      }
      if (homerefset != 1){
        goto imref;  // For Manual- Home Ref to machine at start
      }
        SelectSW = Readswitch(SelectSW); // called function
  }

  switch (SelectSW){
      case 1:
        if(digitalRead((SW1)) == HIGH){ // To recheck for changes
        goto readswref;    // end of all switches to call read switch func
        }
        if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
        lcd.clear();
        lcd.setCursor(0, 3);
        lcd.print("Edit"); 
        }
        goto selector1;
      break;

      case 2:
        if(digitalRead((SW2)) == HIGH){
        goto readswref;    // end of all switches to call read switch func
        }
          if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
        lcd.clear();
        lcd.setCursor(0, 3);
        lcd.print("Auto"); 
        }
        goto selector2;
      break;

      case 3:
        if(digitalRead((SW3)) == HIGH){
        goto readswref;    // end of all switches to call read switch func
        }
        if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
        lcd.clear();
        lcd.setCursor(0, 3);
        lcd.print("Not in use"); 
        }
        goto selector3;
      break;

      case 4:
        if(digitalRead((SW4)) == HIGH){
        goto readswref;    // end of all switches to call read switch func
        }
        if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
          lcd.clear();
          lcd.setCursor(0, 3);
          lcd.print("Jog"); 
        }
        goto selector4;
      break;

      case 5:
        if(digitalRead((SW5)) == HIGH){
        goto readswref;    // end of all switches to call read switch func
        }
        if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
        lcd.clear();
        lcd.setCursor(0, 3);
        lcd.print("JR");
         // job ref point input
        } 
        goto selector5;
      break;

      case 6:
        if(digitalRead((SW6)) == HIGH){
        goto readswref;    // end of all switches to call read switch func
        }
        if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
        lcd.clear();
        lcd.setCursor(0, 3);
        lcd.print("Handle-10X"); 
        encAx=0; // to check the axis selection function
        }
        goto selector6;
      break;

      case 7:
        if(digitalRead((SW7)) == HIGH){
        goto readswref;    // end of all switches to call read switch func
        }
        if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
        lcd.clear();
        lcd.setCursor(0, 3);
        lcd.print("Handle-100X");
        encAx=0; // to check the axis selection function
        } 
        goto selector7;
      break;

      case 8:
        if(digitalRead((SW8)) == HIGH){
        goto readswref;    // end of all switches to call read switch func
        }
        if(OSelectSW != SelectSW){
        OSelectSW = SelectSW;
        lcd.clear();
        lcd.setCursor(0, 3);
        lcd.print("Manual Ref");
        }
        goto selector8;
      break;
    readswref:  // ref position to skip and read switch
    SelectSW = Readswitch(SelectSW);
  }
  goto skipallref; // for end position

  emrgTest: // To do emergency test go to
  {
    if(digitalRead((emrgSW)) == HIGH){
      lcd.setCursor(0, 0);
      lcd.print("Press Rel. - EMG.");
    }
    if(digitalRead((emrgSW)) == LOW){
      delayMicroseconds(debounce);
    if(digitalRead((emrgSW)) == LOW){
      lcd.clear();
      lcd.setCursor(0, 1);
      lcd.print("Emergency test Pass");
      delay (2000);
      lcd.clear();
        emrgt:    // to remain in loop if E.Switch is not released
        if(digitalRead((emrgSW)) == HIGH){
        emrgTestS = 1; // Machine in operation condition
        }
        else {
              lcd.setCursor(0, 1);
              lcd.print("Rel. - EMG.");
        goto emrgt; /// to not come out of loop if button is not released
        }

      lcd.clear();
    }
    }
  }
  goto skipallref; // for end position go to 

  imref: // Function for home ref
  {
      if(homeref2 == 0){
        lcd.setCursor(0, 0);
        lcd.print("Set - Home Ref.");
        lcd.setCursor(0, 1);
        lcd.print("X-Axis-->Home");
        lcd.setCursor(0, 2);
        lcd.print("Z-Axis-->Home");
      }
    // for setting machine to home postion
    if(digitalRead((SW8)) == LOW){
          if(homeref2 == 0){
          homeref2 = 1; // for setting last switch postion
          lcd.setCursor(0, 3);
          lcd.print("Manual Ref       ");
          }
      goto selector8;   // for manual ref of machine to home 1st time
    }
    else {
      lcd.setCursor(0, 3);
      lcd.print("Select Manual Ref");
      homeref2=0; // for resetting home ref to 0 if position unselected
    }    
  }
  goto skipallref; // for end position go to 

  selector1: // For Selection Switch 1 - Edit mode
    {
      char customKey = Akeypad.getKey(); 
      // # Enter, * is ., A , B , C, D
          if (customKey){
            if(customKey != '#'){
              lcd.setCursor(12, 3);
              Keyn1 += customKey;
              lcd.print(Keyn1);
              Serial.println(Keyn1);
              //lcd.print(customKey);
              //Serial.println(customKey);
            }
            else{
            strinp = Keyn1;
            Keyn1 = ""; // reset Keyn1
            lcd.setCursor(12, 3);
            lcd.print("        ");
            goto strkpd; // to use string data
            }
          } 

      lcd.setCursor(0, 0);
      lcd.print(String("A.Cut D   ")+String(cX2)+String("mm "));
      lcd.setCursor(0, 1);
      lcd.print(String("B.Cut L   ")+String(cZ2)+String("mm "));
      lcd.setCursor(0, 2);
      lcd.print(String("C.Splines ")+String(Anos)+String(" "));
      
      // update via serial input e.g A12.45, B234.2, C10
      if(Serial.available()){
        //strinp = Serial.readStringUntil('\n');
        strinp = Serial.readString();
        Serial.print(strinp);

        strkpd: // if string received from key pad
        sa = strinp.charAt(0); // split 1st char p1
        sb = strinp.substring(1); // split 1st char p2
        sv = sb.toFloat();
        //sscanf(strinp, "%c%f", a,&b);
        Serial.println(sa);
        Serial.println(sv);
        lcd.setCursor(10, 3);
        lcd.print(strinp);
        delay(1500);
        lcd.setCursor(10, 3);
        lcd.print("          ");
                
        switch (sa) {
          case 'A':
            if(sv>0 && sv<20){
            cX2 = sv; // update depth of cut 
            EEPROM.put(30, cX2); // update data
            }
            else{
              Serial.println("Not in Range");
              lcd.setCursor(8, 3);
              lcd.print("Out of tol.");
              delay(1500);
              lcd.setCursor(8, 3);
              lcd.print("            ");
            }
          break;

          case 'B':
            if(sv>0 && sv<400){
            cZ2 = sv; // update length of cut 
            EEPROM.put(40, cZ2); // update data
            }
            else{
              Serial.println("Not in Range");
              lcd.setCursor(8, 3);
              lcd.print("Out of tol.");
              delay(1500);
              lcd.setCursor(8, 3);
              lcd.print("            ");
            }
          break;

          case 'C':
            if(sv>0 && sv<=16){
            Anos = sv; // update length of cut
            EEPROM.put(50, Anos); // update data
            } 
            else{
              Serial.println("Not in Range");
              lcd.setCursor(8, 3);
              lcd.print("Out of tol.");
              delay(1500);
              lcd.setCursor(8, 3);
              lcd.print("            ");
            }
          break;
          
          default:
          Serial.println("No match");
          break;
        }
      }
    }
  goto skipallref; // for end position go to 

  selector2: // For Selection Switch 2 - Auto Mode
    {
      if(cyclestatus == 0){     // cycle postion not yet started
          if(digitalRead((cyclestart))==LOW){
            delayMicroseconds(debounce);
          if(digitalRead((cyclestart))==LOW){
            cyclestatus=1;
            // Anoss = 1; // to reset the postion of rotation to zero
            autostp = 0; // to reset cycle start from X0, Z0
              lcd.setCursor(8, 3);
              lcd.print("           "); // clear if programs is restarted
          }
          }
          goto automodenotstarted; // cycle postion was not yet started, to exit steps
      }
      // To check and esnure machine at Home
          lcdupdate();
          delay(1000); 
          mystepperX.setMaxSpeed(2000);
          mystepperZ.setMaxSpeed(2000);
          mystepperX.setAcceleration(1500);
          mystepperZ.setAcceleration(1500);

          cXm= 0- mmX; // move X as Home if not
          sts = cXm * mmstepsX;
          stX=stX + sts;
          mystepperX.moveTo(dirX * stX);
          mystepperX.runToPosition();
          lcdupdate();
          delay(1000);
          
          cZm= 0-mmZ;  // move Z home if not
          sts = cZm * mmstepsZ;
          stZ=stZ + sts;
          mystepperZ.moveTo(dirZ * stZ);
          mystepperZ.runToPosition();
          lcdupdate(); 
          delay(1000);

      // To position X0, Z0 near job
      if (autostp == 0){
          mystepperX.setMaxSpeed(2000);
          mystepperZ.setMaxSpeed(2000);
          mystepperX.setAcceleration(1500);
          mystepperZ.setAcceleration(1500);

          degA = 0; // reset the rotation angle display to zero
          stA=0; // rotation reset
        
        cZm=cX1+cZ0-mmZ;   // position to move Z
        sts = cZm * mmstepsZ;
        stZ=stZ + sts;
        mystepperZ.moveTo(dirZ * stZ);
        mystepperZ.runToPosition();
        autostp = 1; // for next step of auto program
        lcdupdate();
                
        cXm=cX1+cX0-mmX; // position to move X
        sts = cXm * mmstepsX;
        stX=stX + sts;
        mystepperX.moveTo(dirX * stX);
        mystepperX.runToPosition();

        lcdupdate();
      }
      
      autocycle:      // for loop after rotation of axis
      digitalWrite(spdlstart, LOW); // spindle to start
      digitalWrite(clntstart, LOW); // Coolant to start
      // To posstion X1, Z1 job ref point
      if (autostp == 1){
          mystepperX.setMaxSpeed(600); // reduce speed as per pre Feedrate (to program)
          mystepperZ.setMaxSpeed(600);
          mystepperX.setAcceleration(300);
          mystepperZ.setAcceleration(300);

        cXm=cX1-mmX; // position to move X
        sts = cXm * mmstepsX;
        stX=stX + sts;
        mystepperX.moveTo(dirX * stX);
        mystepperX.runToPosition();
        lcdupdate();

        cZm=cZ1-mmZ;   // position to move Z
        sts = cZm * mmstepsZ;
        stZ=stZ + sts;
        mystepperZ.moveTo(dirZ * stZ);
        mystepperZ.runToPosition();
        autostp = 2; // for next step of auto program
        lcdupdate();
      }

      // To posstion X2, Z2 job ref point
      if (autostp == 2){
          mystepperX.setMaxSpeed(100); // reduce speed as per Feedrate (to program)
          mystepperZ.setMaxSpeed(200);
          mystepperX.setAcceleration(100);
          mystepperZ.setAcceleration(200);
        cXm=cX1-mmX-cX2; // position to move X, depth
        sts = cXm * mmstepsX;
        stX=stX + sts;
        mystepperX.moveTo(dirX * stX);
        mystepperX.runToPosition();
        lcdupdate();

        cZm=cZ1-mmZ-cZ2;   // position to move Z, cut size
        sts = cZm * mmstepsZ;
        stZ=stZ + sts;
        mystepperZ.moveTo(dirZ * stZ);
        mystepperZ.runToPosition();
        autostp = 3; // for next step of auto program
        lcdupdate();
      }

        // Move to postion X0, Z0 as home ref 2 before rotating
      if (autostp == 3){
          mystepperX.setMaxSpeed(2000); // Return at High speed
          mystepperZ.setMaxSpeed(2000);
          mystepperX.setAcceleration(1500);
          mystepperZ.setAcceleration(1500);

          cXm=cX1+cX0-mmX; // position to move X home 2
          sts = cXm * mmstepsX;
          stX=stX + sts;
          mystepperX.moveTo(dirX * stX);
          mystepperX.runToPosition();
          lcdupdate();

          digitalWrite(clntstart, HIGH); // Coolant to stop after moving out cutter
          
          cZm=cZ1+cZ0-mmZ;   // position to move Z home 2
          sts = cZm * mmstepsZ;
          stZ=stZ + sts;
          mystepperZ.moveTo(dirZ * stZ);
          mystepperZ.runToPosition();
          autostp = 4; // for next step of auto program
          lcdupdate();
      }
      
      // for rotation of axis
      if (autostp ==4 ){ // for rotation step

        if(Anoss < Anos){
          sts= rstepsA / Anos;
          stA=stA + sts;
          mystepperA.moveTo(stA);
          mystepperA.runToPosition();
          lcdupdate();
          autostp = 1; // to restart from step 1
          Anoss = Anoss + 1 ; // increment the rotation by 1
          goto autocycle; // go start of auto program
        }
          if(Anos!=1){
            sts= rstepsA / Anos;  // for moving to 1 position before cycle end
            stA=stA + sts;
            mystepperA.moveTo(stA);
            mystepperA.runToPosition();
            mystepperA.setCurrentPosition(0); // reset position
            stA=0; // to reset the step
            lcdupdate();
            }

          digitalWrite(spdlstart, HIGH); // spindle stop at program end

          cXm= 0 - mmX; // move X as Home at end of cycle
          sts = cXm * mmstepsX;
          stX=stX + sts;
          mystepperX.moveTo(dirX * stX);
          mystepperX.runToPosition();
          clearRow(0);
          lcdupdate();
          
          cZm= 0 - mmZ;  // move Z home at end of cycle
          sts = cZm * mmstepsZ;
          stZ=stZ + sts;
          mystepperZ.moveTo(dirZ * stZ);
          mystepperZ.runToPosition();
          clearRow(1);
          lcdupdate();  
      
      }

      lcd.setCursor(8, 3);
      lcd.print("Program End");
      delay (9000);
      cyclestatus = 0; // reset cycle status to stop
      Anoss = 1; // to reset the postion of rotation to 1

      /* // Move the motors using G-code commands
      executeGCode(gcodeCommands[currentCommandIndex]);

      // Increment the command index
      currentCommandIndex++;

      // If all commands are executed, reset to the first command
      if (currentCommandIndex >= sizeof(gcodeCommands) / sizeof(gcodeCommands[0])) {
      currentCommandIndex = 0;
      } */

    }
  automodenotstarted:   // if automode is not started exit steps
  goto skipallref; // for end position go to 

  selector3: // For Selection Switch 3 - not in use
  {

  }
  goto skipallref; // for end position go to 

  selector4: // For Selection Switch 4 - Jog Mode
  {
      {
        if(digitalRead((spdlsw))==LOW){
        delayMicroseconds(debounce);
            if(digitalRead((spdlsw))==LOW){
              digitalWrite(spdlstart, LOW);
            }
        }
       else{digitalWrite(spdlstart, HIGH);
       }
      }

      if (digitalRead((JSWXp))==LOW){
        if(Ljsw == 1){
            stX = stX + 8;
            mystepperX.moveTo(dirX * stX);
            while(mystepperX.distanceToGo() != 0){ // loop for pending step
              mystepperX.runSpeedToPosition();
            }
        }
        else {
          delayMicroseconds(debounce);
          if (digitalRead((JSWXp))==LOW){
          Ljsw =  1;
          }
        }
        goto jogrf;
      }
      if (digitalRead((JSWXm))==LOW){
        if(Ljsw == 2){
          stX = stX -8;
          mystepperX.moveTo(dirX * stX);
          while(mystepperX.distanceToGo() != 0){ // loop for pending step
            mystepperX.runSpeedToPosition();
          }
        }
        else {
          delayMicroseconds(debounce);
          if (digitalRead((JSWXm))==LOW){
          Ljsw =  2;
          }
        }
        goto jogrf;
      }

      if (digitalRead((JSWZp))==LOW){
        if(Ljsw == 3){
          stZ = stZ + 10;
          mystepperZ.moveTo(dirZ * stZ);
          while(mystepperZ.distanceToGo() != 0){ // loop for pending step
            mystepperZ.runSpeedToPosition();
          }
        }
        else {
          delayMicroseconds(debounce);
          if (digitalRead((JSWZp))==LOW){
          Ljsw =  3;
          }
        }
        goto jogrf;
      }
      if (digitalRead((JSWZm))==LOW){
        if(Ljsw == 4){
          stZ = stZ - 10;
          mystepperZ.moveTo(dirZ * stZ);
          while(mystepperZ.distanceToGo() != 0){ // loop for pending step
            mystepperZ.runSpeedToPosition();
          }
        }
        else {
          delayMicroseconds(debounce);
          if (digitalRead((JSWZm))==LOW){
          Ljsw =  4;
          }
        }
        goto jogrf;
      }

      if (digitalRead((JSWAp))==LOW){
        if(Ljsw == 5){
          stA = stA + 10;
          mystepperA.moveTo(stA);
          while(mystepperA.distanceToGo() != 0){ // loop for pending step
            mystepperA.runSpeedToPosition();
          }
        }
        else {
          delayMicroseconds(debounce);
          if (digitalRead((JSWAp))==LOW){
          Ljsw =  5;
          }
        }
        goto jogrf;
      }
      if (digitalRead((JSWAm))==LOW){
        if(Ljsw == 6){
          stA = stA - 10;
          mystepperA.moveTo(stA);
          while(mystepperA.distanceToGo() != 0){ // loop for pending step
            mystepperA.runSpeedToPosition();
          }
        }
        else {
          delayMicroseconds(debounce);
          if (digitalRead((JSWAm))==LOW){
          Ljsw =  6;
          }
        }
        goto jogrf;
      }
    }
      lcdupdate();
      jogrf: // to skip to end without lcd update till jog botton pressed
  goto skipallref; // for end position go to 

  selector5: // For Selection Switch 3 - job ref point setting Mode
    {
      if (digitalRead((jobref))==LOW){
      jbrefX = mmX; // set the job ref postion by moving m/c
      jbrefZ = mmZ; // set the job ref position by moving m/c
      EEPROM.put(10, jbrefX); //for storing the position
      EEPROM.put(20, jbrefZ); // for storing the position
      cX1 = jbrefX; // job ref from home - duplicate var
      cZ1 = jbrefZ; // job ref from home - duplicate var
      }
     // for lcd display update
      lcdupdate();
        // for jb ref setting display
        lcd.setCursor(11, 2);
        lcd.print(String("C")+String(jbrefX));
        lcd.setCursor(11, 3);
        lcd.print(String("D")+String(jbrefZ));
      
      // for input from key pad
      char customKey = Akeypad.getKey(); 
      // # Enter, * is ., A , B , C, D
          if (customKey){
            if(customKey != '#'){
              lcd.setCursor(3, 3);
              Keyn1 += customKey;
              lcd.print(Keyn1);
              Serial.println(Keyn1);
              //lcd.print(customKey);
              //Serial.println(customKey);
            }
            else{
            strinp = Keyn1;
            Keyn1 = ""; // reset Keyn1
            lcd.setCursor(3, 3);
            lcd.print("       ");
            goto strkpd2; // to use string data
            }
          } 

    // update via serial input only -ve e.g C-1040.45, D-134.2,
        if(Serial.available()){
        strinp = Serial.readString();
        Serial.print(strinp);

        strkpd2: // for inptu from key pad
        sa = strinp.charAt(0); // split 1st char p1
        sb = strinp.substring(1); // split 1st char p2
        sv = sb.toFloat();
          if(sv>0){
          sv= -sv; //to covert to -ve for ref from home
          }
        Serial.println(sa);
        Serial.println(sv);
        lcd.setCursor(3, 3);
        lcd.print(strinp);
        delay(1500);
        lcd.setCursor(3, 3);
        lcd.print("         ");
                
        switch (sa) {
          case 'C':
            if(sv>-400 && sv<0){ // for X axis
            jbrefX = sv; // update job ref
            EEPROM.put(10, jbrefX); //for storing the position
            cX1 = jbrefX; // job ref from home - duplicate var
            }
            else{
              Serial.println("Not in Range");
              lcd.setCursor(4, 3);
              lcd.print("Tol. Out");
              delay(1500);
              lcd.setCursor(4, 3);
              lcd.print("        ");
            }
          break;

          case 'D':
            if(sv>-1200 && sv<0){ // for Z axis
            jbrefZ = sv; // update position
            EEPROM.put(20, jbrefZ); // for storing the position
            cZ1 = jbrefZ; // job ref from home - duplicate var
            }
            else{
              Serial.println("Not in Range");
              lcd.setCursor(4, 3);
              lcd.print("Tol. Out");
              delay(1500);
              lcd.setCursor(4, 3);
              lcd.print("        ");
            }
          break;
         
          default:
          Serial.println("No match");
          break;
        }
      }

    }
  goto skipallref; // for end position go to 
  
  selector6: // For Selection Switch 6- Handle Mode 10X
    {
      // Read encode
      encPP = encP;
      encN = digitalRead(encC);
      Serial.println(encN);
        if ((encN== LOW)) {
          if ((digitalRead(encD) == HIGH)) {
          encP--;
          } else {
          encP++; 
          }
          encL=encN;
        // lcd.setCursor(14, 2);
        // lcd.print(encP);
          switch (encAx){
            case 1:
              encstp1X = (encP - encPP) * mmstepsX/10;                 
              stX = stX + encstp1X;
              mystepperX.moveTo(dirX * stX);
              mystepperX.runSpeedToPosition();
            break;
            case 2:
              encstp1X = (encP - encPP) * mmstepsZ/10;  
              stZ = stZ + encstp1X;
              mystepperZ.moveTo(dirZ * stZ);
              mystepperZ.runSpeedToPosition();
            break;
            case 3:
              encstp1X = (encP - encPP) * rstepsA/10/360;  
              stA = stA + encstp1X;
          mystepperA.moveTo(stA);
          mystepperA.runSpeedToPosition();
            break;
          }
        goto skipallref; // for end position go to 
        }
       // lcdupdate();
      encAx = ReadAxis(encAx); // Called Function when encoder idle
    }
  goto skipallref; // for end position go to 

  selector7: // For Selection Switch 7 - Handle Mode 100X
    {
      // Read encode
      encPP = encP;
      encN = digitalRead(encC);
        if ((encN !=encL)) {
          if ((digitalRead(encD) == LOW)&& (encN == LOW)) {
          encP--;
          } else {
          encP++; 
          }
          encL=encN;
        // lcd.setCursor(14, 2);
        // lcd.print(encP);
          switch (encAx){
            case 1:
              encstp1X = (encP - encPP) * mmstepsX/1;                 
              stX = stX + encstp1X;
              mystepperX.moveTo(dirX * stX);
              mystepperX.runSpeedToPosition();
            break;
            case 2:
              encstp1X = (encP - encPP) * mmstepsZ/1;  
              stZ = stZ + encstp1X;
              mystepperZ.moveTo(dirZ * stZ);
              mystepperZ.runSpeedToPosition();
            break;
            case 3:
              encstp1X = (encP - encPP) * rstepsA/1/360;  
              stA = stA + encstp1X;
          mystepperA.moveTo(stA);
          mystepperA.runSpeedToPosition();
            break;
          }
        goto skipallref; // for end position go to 
        }
       // lcdupdate();
      encAx = ReadAxis(encAx); // Called Function when encoder idle
    }
  goto skipallref; // for end position go to 

  selector8: // For Selection Manual home ref
    {
       if (digitalRead((jobref))== LOW){
          delayMicroseconds(debounce);
          if (digitalRead((jobref))== LOW){
       //if (digitalRead((lmtX1))== HIGH){ // for sensor home ref
          //delayMicroseconds(debounce);
          //if (digitalRead((lmtX1))== HIGH){
            hrX = 1; // home ref set
            mmX = 0;
            stX = 0;
            mystepperX.setCurrentPosition(0); // reset position
            lcd.setCursor(0, 1);
            lcd.print("X-Axis-->Home *");
          }
        }
        else{
          HomeJogX ();
        }
      if (digitalRead((jobref))== LOW){
          delayMicroseconds(debounce);
          if (digitalRead((jobref))== LOW){
      //if (digitalRead((lmtZ1))== HIGH){ // for sensor home ref
          //delayMicroseconds(debounce);
          //if (digitalRead((lmtZ1))== HIGH){
              hrZ = 1; // home ref set
              mmZ = 0;
              stZ = 0;
              mystepperZ.setCurrentPosition(0); // reset position
              lcd.setCursor(0, 2);
              lcd.print("Z-Axis-->Home *");
          }
      }
        else{
          HomeJogZ ();
        }

      if ( (hrX == 1) && (hrZ == 1)){ // test after machine start
          if ( (stX == 0) && (stZ == 0)){ // 
            delay (1000);
            lcd.clear();
            lcd.setCursor(0, 3);
            lcd.print("Machine Home OK");
            degA = 0;
            stA=0;
            delay (2000);
            lcd.clear();
            homerefset = 1 ; // as home ref is set
          }
      }
    }
  skipallref:
  {}
}

// Function section
// LCD update Function for Axis Value
void lcdupdate () {
    mmX = stX/mmstepsX;
    mmZ = stZ/mmstepsZ;
    degA = (stA*360)/rstepsA;
    if(degA <0){
    degA=degA+360; // for temp correction below 0 deg
    }
    if(degA >360){
    degA=degA-360; // for temp correction above 360 deg
    }
    // for relative position
    relX=mmX-jbrefX;  // relative postion from job ref
    relZ=mmZ-jbrefZ; // relative position from job ref
// For X axis distance //
  lcd.setCursor(0, 0);
  lcd.print(String("X:")+String(mmX,2)+String("mm "));
  // For Z axis distance //+String(relX,2)+String("  ")
  lcd.setCursor(0, 1);
  lcd.print(String("Z:")+String(mmZ,2)+String("mm "));
    // For A axis distance //+String(relZ,2)+String("  ")
  lcd.setCursor(0, 2);
  lcd.print(String("A:") + String(degA, 1) + String("Deg "));


      lcd.setCursor(12, 0);
      lcd.print(String(relX,2)+String(" "));
      lcd.setCursor(12, 1);
      lcd.print(String(relZ,2)+String(" "));
}

// Read axis switch for Handle mode
int ReadAxis(int encAx)
{
    if ((digitalRead((JSWXp)) == LOW)||(digitalRead((JSWXm)) == LOW)){
      if(encAx != 1){
      lcd.setCursor(14, 3);
      lcd.print("X-Axis");
      encAx = 1; // for X Axis 
      }
    }
    if ((digitalRead((JSWZp)) == LOW)||(digitalRead((JSWZm)) == LOW)){
      if(encAx != 2){
      lcd.setCursor(14, 3);
      lcd.print("Z-Axis");
      encAx = 2; // for Z Axis
      }
     }
    if ((digitalRead((JSWAp)) == LOW)||(digitalRead((JSWAm)) == LOW)){
      if(encAx != 3){
      lcd.setCursor(14, 3);
      lcd.print("A-Axis");
      encAx = 3; // for A Axis 
      }
    }
    return encAx;
}

// LCD Row Clear function 
void clearRow(byte rowToClear)
{
  lcd.setCursor(0, rowToClear);
  lcd.print("                    ");  // Print 20 spaces in row
}

// Read Switch function
int Readswitch(int SelectSW){

    if(digitalRead((SW1)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW1)) == LOW){
            SelectSW = 1;  // For Edit mode
          }
        }
    else if(digitalRead((SW2)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW2)) == LOW){
            SelectSW = 2;  // For auto mode
          }
        }
    else if(digitalRead((SW3)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW3)) == LOW){
            SelectSW = 3;  // For job ref mode
          }
        }
    else if(digitalRead((SW4)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW4)) == LOW){
            SelectSW = 4;  // For Job mode
          }
        }
    else if(digitalRead((SW5)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW5)) == LOW){
            SelectSW = 5;  // For Handle mode 1X
          }
        }
    else if(digitalRead((SW6)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW6)) == LOW){
            SelectSW = 6;  // For Handle mode 10X
          }
        }
    else if(digitalRead((SW7)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW7)) == LOW){
            SelectSW = 7;  // For handle mode 100X
          }
        }
    else if(digitalRead((SW8)) == LOW)
        {
          delayMicroseconds(debounce);
          if(digitalRead((SW8)) == LOW){
            SelectSW = 8;  // Test run move , to change to manual ref
          }
        } 
    return SelectSW; // for return value
}

// For intial JOG for home position
void HomeJogX (){

      if (digitalRead((JSWXp))==LOW){
        digitalWrite(23, bdirX);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval2){ // Diff speed
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(22, stepState);
        }
      }
      if (digitalRead((JSWXm))==LOW){
        digitalWrite(23, !bdirX);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval2){// Diff speed
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(22, stepState);
        }
      }
}

// For intial JOG for home position
void HomeJogZ (){

      if (digitalRead((JSWZp))==LOW){
        digitalWrite(25, bdirZ);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval2){// Diff speed
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(24, stepState);
        }
      }
      if (digitalRead((JSWZm))==LOW){
        digitalWrite(25, !bdirZ);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval2){// Diff speed
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(24, stepState);
        }
      }
}