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

LiquidCrystal_I2C lcd(0x27, 20, 4); //LCD communication 20 x 4
// for selector swith
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 = 10; // (milliseconds) half-interval for stepping
unsigned long interval2 = 2; // (milliseconds) half-interval  home ref jog
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

// 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 autostp=0; // for use in loop for step of loop in auto mode
float cX0= -200; // for ref home 2 position near job to set in EEPROM
float cZ0= -500;  // for ref home 2 position near job to set in EEPROM
float cX1= -210; // for ref position 0 of job to set in EEPROM
float cZ1=-505;  // for ref position 0 of job to set in EEPROM
float cA1 =0; // for ref position 0 of job
float cX2 =12; // for depth of cut to set in EEPROM
float cZ2 =150; // for depth of cut to set in EEPROM
int Anos=10; // No of splines to set in EEPROM
int Anoss = 0; // Index postion of rotation
float fdrX= 2; // feed rate depth
float fdrZ = 5; // feed rate side wise

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

int currentCommandIndex = 0;

// For Stepper Controller //
#include <Stepper.h>
#include <AccelStepper.h>
/* Stepper 1/X 
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 mmstepsX = 156;
/* Stepper 2/Z 
steps per rev 400 (full step setting), 5 mm per rev ball screw
mmstepsZ=400/5 = 80*/
const float mmstepsZ = 80;
/* Stepper 3/A 
steps per rev 400 (full step setting), gear ratio 60
DegstepsA=400*60/360 = 66.66667*/
const float degstepsA = 400*60/360;
  // For stepper // not in use //
  // Stepper myStepperX(mmstepsX, 22, 23);
  // Stepper myStepperZ(mmstepsZ, 24, 25);
  // Stepper myStepperA(degstepsA, 26, 27);
// Define stepper driver pins//
AccelStepper mystepperX(AccelStepper::DRIVER, 22, 23);
AccelStepper mystepperZ(AccelStepper::DRIVER, 24, 25);
AccelStepper mystepperA(AccelStepper::DRIVER, 26, 27);

// For stepper position and movement
float absX = 999.999; // Absolute position of X //
float absZ = 999.999; // Absolute position of Z //
float mmX; // Actual position of X //
float mmZ; // Actual 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 //
long sthX; // Half Steps Number X // not to be used
long sthZ; // Half Steps Number Z // not to be used
long sthA; // Half Steps Number A // not to be used
float sts=100; // Step to move in each loop //

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

    //initialize lcd screen //
  lcd.begin(16,4);
  // turn on the backlight //
  lcd.backlight();
    // Stepper library // Not in use
    // Stepper speed in mm/Degree per min//
      //myStepperX.setSpeed(120);
      //myStepperZ.setSpeed(120);
      //myStepperA.setSpeed(720);
  mystepperX.setMaxSpeed(2000);
  mystepperZ.setMaxSpeed(2000);
  mystepperA.setMaxSpeed(2000);
  mystepperX.setAcceleration(1000);
  mystepperZ.setAcceleration(1000);
  mystepperA.setAcceleration(1000);

// For Max movement //
  mmXmv = 6.2;
  mmZmv= 300.4;
  degAmv = 36.2;
  // For Emergency Switch interrupt

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

  attachInterrupt(digitalPinToInterrupt(2), EInterrupt, FALLING);
}

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

void loop() {

  if (emrgTestS == 2){ // If emergency button intrupt
        goto emrg;  // go to emergency section 
      }

  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("JOB Ref. Point");
        }
        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("Handle-1X");
        encAx=0; // to check the axis selection function
        } 
        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){
      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 

        
  emrg: // Emergency section
    {   
        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 go to 


  imref: // Function for home ref
  {
      if(homeref2 == 0){
        lcd.setCursor(0, 0);
        lcd.print("Manual - 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
    }
    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
    {
      
         /* lcd.setCursor(3, 2);
          lcd.print(customKey);
          Serial.println(customKey);*/

      char customKey = Akeypad.getKey(); 
      // # Enter, * Clear, A -Up, B-Down, C-Left, D-Right
  
        if (customKey){
          lcd.setCursor(3, 2);
          lcd.print(customKey);
          Serial.println(customKey);
        }
      
    }
  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){
            cyclestatus=1;
          }
          goto automodenotstarted; // cycle postion was not yet started, to exit steps
      }
      // To posstion X0, Z0
      if (autostp == 0){
          mystepperX.setMaxSpeed(2000);
          mystepperZ.setMaxSpeed(2000);
          mystepperX.setAcceleration(1000);
          mystepperZ.setAcceleration(1000);
        cXm=cX0-mmX; // position to move X
        sts = cXm * mmstepsX;
        sthX = sthX + 2*sts;
        stX=sthX/2;

        cZm=cZ0-mmZ;   // position to move Z
        sts = cZm * mmstepsZ;
        sthZ = sthZ + 2*sts;
        stZ=sthZ/2;
        mystepperX.moveTo(stX);
        mystepperZ.moveTo(stZ);
        mystepperX.runToPosition();
        mystepperZ.runToPosition();
        autostp = 1; // for next step of auto program
        lcdupdate();
      }
      
      autocycle:      // for loop after rotation of axis
      // To posstion X1, Z1 job ref point
      if (autostp == 1){
          mystepperX.setMaxSpeed(600); // reduce speed as per Feedrate (to program)
          mystepperZ.setMaxSpeed(600);
          mystepperX.setAcceleration(300);
          mystepperZ.setAcceleration(300);
        cXm=cX1-mmX; // position to move X
        sts = cXm * mmstepsX;
        sthX = sthX + 2*sts;
        stX=sthX/2;

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


      // To posstion X2, Z2 job ref point
      if (autostp == 2){
        cXm=cX1-mmX-cX2; // position to move X, depth
        sts = cXm * mmstepsX;
        sthX = sthX + 2*sts;
        stX=sthX/2;

        cZm=cZ1-mmZ-cZ2;   // position to move Z, cut size
        sts = cZm * mmstepsZ;
        sthZ = sthZ + 2*sts;
        stZ=sthZ/2;
        mystepperX.moveTo(stX);
        mystepperZ.moveTo(stZ);
        mystepperX.runToPosition();
        //lcdupdate(); // not working
        mystepperZ.runToPosition();
        autostp = 3; // for next step of auto program
        lcdupdate();
      }
        
        // Move to postion X0, Z0 before rotating
      if (autostp == 3){
          mystepperX.setMaxSpeed(2000); // Return at High speed
          mystepperZ.setMaxSpeed(2000);
          mystepperX.setAcceleration(1000);
          mystepperZ.setAcceleration(1000);
          cXm=cX0-mmX; // position to move X
          sts = cXm * mmstepsX;
          sthX = sthX + 2*sts;
          stX=sthX/2;

          cZm=cZ0-mmZ;   // position to move Z
          sts = cZm * mmstepsZ;
          sthZ = sthZ + 2*sts;
          stZ=sthZ/2;
          mystepperX.moveTo(stX);
          mystepperZ.moveTo(stZ);
          mystepperX.runToPosition();
          // lcdupdate(); // not working
          mystepperZ.runToPosition();
          autostp = 4; // for next step of auto program
          lcdupdate();
      }
      
      // for rotation of axis
      
      for ( int Anoss =0; Anoss <  Anos; Anoss ++){
        sts= 360 * degstepsA / Anos;
        sthA = sthA + 2*sts;
        stA=sthA/2;
        mystepperA.moveTo(stA);
        mystepperA.runToPosition();
        lcdupdate();
        autostp = 1; // to restart from step 1
        if(Anoss==9){
          goto cycleends; // exists after cycle complete
        }
      goto autocycle; // go start of auto program
      }

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

      /* // 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 - job ref - MDI Mode
    {
            // Not in use //
    /* myStepperX.step(20);
    myStepperZ.step(10);
    myStepperA.step(5); */
      if (mmXmv > mmX){
        sthX = sthX + 2*sts;
        stX=sthX/2;
      //mmX = mmX + sts/mmstepsX;
      }
      if (mmZ < mmZmv){
        sthZ = sthZ + 2*sts;
        stZ=sthZ/2;
        //mmZ = mmZ + sts/mmstepsZ;
        }
      if (degA < degAmv){
        sthA = sthA + 2*sts;
        stA=sthA/2;
      //degA=degA+sts/degstepsA;
      } 
      mystepperX.moveTo(stX);
      mystepperZ.moveTo(stZ);
      mystepperA.moveTo(stA);
      mystepperX.runToPosition();
      mystepperZ.runToPosition();
      mystepperA.runToPosition();
    // for lcd display update
      lcdupdate();

    }
  goto skipallref; // for end position go to 
  

  selector4: // For Selection Switch 4 - Jog Mode
    {
      if (digitalRead((JSWXp))==LOW){
      digitalWrite(23, HIGH);
      unsigned long currentMillis = millis(); // store the time
      if (currentMillis - previousMillis >= interval){
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(22, stepState);
          sthX = sthX + 1;
      }
        goto jogrf;
      }
      if (digitalRead((JSWXm))==LOW){
        digitalWrite(23, LOW);
      unsigned long currentMillis = millis(); // store the time
      if (currentMillis - previousMillis >= interval){
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(22, stepState);
          sthX = sthX -1;
      }
        goto jogrf;
      }

      if (digitalRead((JSWZp))==LOW){
          digitalWrite(25, HIGH);
      unsigned long currentMillis = millis(); // store the time
      if (currentMillis - previousMillis >= interval){
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(24, stepState);
          sthZ = sthZ + 1;
      }
        goto jogrf;
      }
      if (digitalRead((JSWZm))==LOW){
        digitalWrite(25, LOW);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval){
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(24, stepState);
          sthZ= sthZ-1;
        }
        goto jogrf;
      }

      if (digitalRead((JSWAp))==LOW){
        digitalWrite(27, HIGH);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval){
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(26, stepState);
          sthA = sthA +1;
        }
        goto jogrf;
      }
      if (digitalRead((JSWAm))==LOW){
        digitalWrite(27, LOW);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval){
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(26, stepState);
          sthA = sthA -1;
        }
        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 5- Handle Mode 1X
    {
      // 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);
        }

      encAx = ReadAxis(encAx); // Called Function
        switch (encAx){
          case 1:
            encstp1X = (encP - encPP) * mmstepsX/100;                 
            sthX = sthX + encstp1X;
            stX = sthX/2;
            mystepperX.moveTo(stX);
            mystepperX.runToPosition();
          break;
          case 2:
            encstp1X = (encP - encPP) * mmstepsZ/100;  
            sthZ = sthZ + encstp1X;
            stZ = sthZ/2;
            mystepperZ.moveTo(stZ);
            mystepperZ.runToPosition();
          break;
          case 3:
            encstp1X = (encP - encPP) * degstepsA/100;  
            sthA = sthA + encstp1X;
            stA = sthA/2;
        mystepperA.moveTo(stA);
        mystepperA.runToPosition();
          break;
        }
        /*if (encP != encPP){
        goto skipallref; // for end position go to 
        }  
        lcdupdate();*/
    }
goto skipallref; // for end position go to 


  selector6: // For Selection Switch 6- Handle Mode 10X
    {
        // Read encode
      encPP = encP;
      encN = digitalRead(encC);
        if ((encN !=encL)) {
          if ((digitalRead(encD) == LOW)&& (encN == LOW)) {
          encP--;
          } else {
          encP++;
          encL=encN;    
          }
        }

      encAx = ReadAxis(encAx); // Called Function
        switch (encAx){
          case 1:
            encstp1X = (encP - encPP) * mmstepsX/10;                 
            sthX = sthX + encstp1X;
            stX = sthX/2;
            mystepperX.moveTo(stX);
            mystepperX.runToPosition();
          break;
          case 2:
            encstp1X = (encP - encPP) * mmstepsZ/10;  
            sthZ = sthZ + encstp1X;
            stZ = sthZ/2;
            mystepperZ.moveTo(stZ);
            mystepperZ.runToPosition();
          break;
          case 3:
            encstp1X = (encP - encPP) * degstepsA/10;  
            sthA = sthA + encstp1X;
            stA = sthA/2;
        mystepperA.moveTo(stA);
        mystepperA.runToPosition();
          break;
        }      
        // lcdupdate();    
    }
  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;    
          }
        }

      encAx = ReadAxis(encAx); // Called Function
        switch (encAx){
          case 1:
            encstp1X = (encP - encPP) * mmstepsX/1;                 
            sthX = sthX + encstp1X;
            stX = sthX/2;
            mystepperX.moveTo(stX);
            mystepperX.runToPosition();
          break;
          case 2:
            encstp1X = (encP - encPP) * mmstepsZ/1;  
            sthZ = sthZ + encstp1X;
            stZ = sthZ/2;
            mystepperZ.moveTo(stZ);
            mystepperZ.runToPosition();
          break;
          case 3:
            encstp1X = (encP - encPP) * degstepsA/1;  
            sthA = sthA + encstp1X;
            stA = sthA/2;
        mystepperA.moveTo(stA);
        mystepperA.runToPosition();
          break;
        }      
        // lcdupdate();    
    
    }
  goto skipallref; // for end position go to 


  selector8: // For Selection Manual home ref
    {
       if (digitalRead((lmtX1))== HIGH){
        absX = 0;
        mmX = 0;
        stX = 0;
        sthX = 0;
        lcd.setCursor(0, 1);
        lcd.print("X-Axis-->Home *");
        }
        else{
          HomeJogX ();
        }
      if (digitalRead((lmtZ1))== HIGH){
        absZ = 0;
        mmZ = 0;
        stZ = 0;
        sthZ = 0;
        lcd.setCursor(0, 2);
        lcd.print("Z-Axis-->Home *");
        }
        else{
          HomeJogZ ();
        }

      if ( (absX == 0) && (absZ == 0)){ // 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;
            sthA=0;
            delay (2000);
            lcd.clear();
            homerefset = 1 ; // as home ref is set
          }
      }
    }
  skipallref:
  {}
}


// Function section
// LCD update Function for Axis Value
void lcdupdate ()
//(long &stX, long &stZ, long &stA, float &mmX, float &mmZ, float &degA)
{
    stX = sthX/2;
    stZ = sthZ/2;
    stA = sthA/2;
    mmX = stX/mmstepsX;
    mmZ = stZ/mmstepsZ;
    degA = stA/degstepsA; 
// For X axis distance //
  lcd.setCursor(0, 0);
  lcd.print("X: ");
  lcd.setCursor(3, 0);
  lcd.print(mmX);
  // For Z axis distance //
  lcd.setCursor(0, 1);
  lcd.print("Z: ");
  lcd.setCursor(3, 1);
  lcd.print(mmZ);
    // For Z axis distance //
  lcd.setCursor(0, 2);
  lcd.print("A: ");
  lcd.setCursor(8, 2);
  lcd.print("Deg");
  lcd.setCursor(3, 2);
  lcd.print(degA,1);
  //lcd.setCursor(10,3);
  //lcd.print(stX);
}


// 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)
        {
          SelectSW = 1;  // For Edit mode
        }
    else if(digitalRead((SW2)) == LOW)
        {
          SelectSW = 2; // for auto mode
        }
    else if(digitalRead((SW3)) == LOW)
        {
          SelectSW = 3; // Blank - MDI mode
        }
    else if(digitalRead((SW4)) == LOW)
        {
          SelectSW = 4; // Jog Mode
        }
    else if(digitalRead((SW5)) == LOW)
        {
          SelectSW = 5; // Handle Mode 1X
        }
    else if(digitalRead((SW6)) == LOW)
        {
          SelectSW = 6; // Handle Mode 10X
        }
    else if(digitalRead((SW7)) == LOW)
        {
          SelectSW = 7; // Handle Mode 100X
        }
    else 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, HIGH);
        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, LOW);
        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, HIGH);
        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, LOW);
        unsigned long currentMillis = millis(); // store the time
        if (currentMillis - previousMillis >= interval2){// Diff speed
          previousMillis = currentMillis; // store last step time
          stepState = !stepState;
          digitalWrite(24, stepState);
        }
      }

    }