//------------------------------- librarys ----------------------------------
#include <Wire.h>
//#include <rgb_lcd.h>
//#include <LiquidCrystal.h>
#include <Servo.h>
#include <Keypad.h>
#include <Keypad_I2C.h>
#include "AccelStepper.h" 
//------------------------------- lcd ----------------------------------
//rgb_lcd lcd;
#include <LiquidCrystal_I2C.h>
//LiquidCrystal_I2C lcd(0x3F,20,4);  // set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
//------------------------------- stepper ----------------------------------
//#define stepPin 49
//#define dirPin 50
#define stepPin 5
#define dirPin 6
#define enablePin 53
#define motorInterfaceType 1

// Stepper Travel ariables
long TravelX=600;  // Used to store the X value entered in the Serial Monitor
int move_finished=1;  // Used to check if move is completed
long initial_homing=-1;  // Used to Home Stepper at startup
AccelStepper stepperX(motorInterfaceType, stepPin, dirPin);
//------------------------------- servo ----------------------------------
Servo snippers;
#define servo 46
#define openAngle 90
#define closedAngle 0

//------------------------------ keypad input ----------------------------

#define I2CADDR 0x20

const byte ROWS = 4; //four rows
const byte COLS = 4; //four columns
//define the cymbols on the buttons of the keypads
char keys[ROWS][COLS] = {
   {'1', '2', '3', 'A'},
  {'4', '5', '6', '.' },
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {0, 1, 2, 3}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {4, 5, 6, 7}; //connect to the column pinouts of the keypad

//initialize an instance of class NewKeypad
Keypad_I2C keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS, I2CADDR); 

char ButtonState = 0;

#define START '*'   //Maps Character inputs to logical Button Names
#define STOP  '#'
#define F1    'A'
#define F2    'B'
#define F3    'C'
#define F4    'D'
#define NOKEY  0
#define LEFT    '4'
#define RIGHT   '6'
#define UP      '2'
#define DOWN    '5'

//----------------------inputs outputs ----------------------------------

#define termatiko A2
#define home_switch A3 

#define mcutoff 4
#define mcut 7
#define Cutter 8
#define Mpos 12
//------------------------------- user settings ----------------------------------
float wireLength = 0.0;
float OldwireLength= 0.0;
float previouswireLength = 0.0;
unsigned int wireQuantity = 0;
int cuttime = 4000;
int positiontime = 2000;
int Maintimer =2000;
int Homed =1000;
int steptime=150;

bool isDecimal = false;   // A flag to indicate if the decimal point has been pressed
int decimalPart = 0;      // The decimal part of the length
int decimalPlace = 1;     // To keep track of the position of the decimal place

//------------------------------- system settings ----------------------------------
int state =6;
int incrementSpeed = 0;
int previousWireLength = 0.0;
int previousWireQuantity = 0;
//float mmPerStep = 0.00625;
float mmPerStep = 6.25;

void setup() {
  keypad.begin( );
  Serial.begin(9600);
  lcd.init();                      // initialize the lcd 
  lcd.backlight();
  pinMode(stepPin,OUTPUT); 
  pinMode(dirPin,OUTPUT);
  pinMode(enablePin,OUTPUT);
  pinMode(Mpos,OUTPUT); 
  pinMode(mcut,OUTPUT);
  pinMode(mcutoff,OUTPUT);
  pinMode(Cutter,OUTPUT);
  
  pinMode(home_switch, INPUT_PULLUP);
  pinMode(termatiko, INPUT_PULLUP);

   //  Set Max Speed and Acceleration of each Steppers at startup for homing
   stepperX.setMaxSpeed(20000.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
   stepperX.setAcceleration(10000.0);  // Set Acceleration of Stepper
  
}

void loop() {

   
  if (ButtonState == STOP){
    if(state == 3){
      state = 0;
    }
    else{
      state += 1;
    }
    delay(100);
    lcd.clear();
  }
  if (ButtonState == START && state > 0 && state < 2){
    state -=1;
    delay(100);
    lcd.clear();
  }
   ButtonState = keypad.getKey();

  switch (state){


    case 0:
      handleLengthInput();
      break;
    case 1:
      confirm();
      break;
    case 2:
      currentlyCutting();
      break;
    case 3:
      confirm();
      break;
    case 4:
      currentlyCutting();
      break;
    case 5:
      finishedCutting();
      break;
    case 6:
      Homing();
      break;
    case 7:
      MDI();
      break;
  }
 

}

void MDI(){
   while (digitalRead(home_switch)) { // Make the Stepper move CW until the switch is deactivated
    stepperX.moveTo(initial_homing);  
    stepperX.run();
    initial_homing++;
    delay(5);
  }
}

void homeScreen(){
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("VALTZAKIS ");
  lcd.setCursor(11, 0);
  lcd.print("CNC ");
  lcd.setCursor(0, 1);
  lcd.print("MACHINERY ");
  lcd.setCursor(11, 1);
  lcd.print("NEXT>");
  delay(100);
  
  
}
 void Homing() {
     lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("PATHSE C GIA ");
      lcd.setCursor(0, 1);
      lcd.print("MHDENISMO");
      delay(100);
    char key = keypad.getKey();
  
    if (key == 'C')
     {
     
     //Serial.print("Stepper is Homing . . . . . . . . . . . ");
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("DIADIKASIA");
      lcd.setCursor(0, 1);
      lcd.print("MHDENISMOU");
  
      
    while (!digitalRead(home_switch)) {  // Make the Stepper move CCW until the switch is activated   
      stepperX.moveTo(initial_homing);  // Set the position to move to
      initial_homing--;  // Decrease by 1 for next move if needed
      stepperX.run();  // Start moving the stepper
        delayMicroseconds(steptime);
    }
//    stepperX.setCurrentPosition(0);  // Set the current position as zero for now
//    stepperX.setMaxSpeed(5000.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
//    stepperX.setAcceleration(2000.0);  // Set Acceleration of Stepper
//    initial_homing=1;
  
    while (digitalRead(home_switch)) { // Make the Stepper move CW until the switch is deactivated
      stepperX.moveTo(initial_homing);  
      stepperX.run();
      initial_homing++;
      delayMicroseconds(steptime);
    }
     stepperX.setCurrentPosition(0);
//     CurrentLocation = 0.0;
    //Serial.println("Homing Completed");
   // Serial.println("");
    stepperX.setMaxSpeed(20000.0);      // Set Max Speed of Stepper (Faster for regular movements)
    stepperX.setAcceleration(10000.0);  // Set Acceleration of Stepper
    state=0;
   }
 }
void chooseWireQuantity(){
  wireQuantity = changeValue(wireQuantity);

  //clear LCD if required
  if(previousWireQuantity != wireQuantity){
    lcd.clear();
    previousWireQuantity = wireQuantity;
  }
 
  //Display information on LCD
  lcd.setCursor(0, 0);
  lcd.print("POSOTHTA:" + (String)wireQuantity);
  displayNavigation();
}
void handleLengthInput() {

  char key = keypad.getKey();

  if (wireLength < 0.0 || wireLength > 3600.0) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("EKTOS ORIWN");
    delay(1000);
    //lcd.setCursor(0, 0);
    //lcd.print("EISAGETE");
    lcd.setCursor(0, 1);
    lcd.print("0 to 3600 mm");
    delay(2000);

    wireLength = 0.0;
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("NEW: " + String(wireLength, 1) + " mm");
    lcd.setCursor(0,1 );
    lcd.print("OLD: " + String(OldwireLength, 1) + " mm");
    delay(100);
  } else {
    lcd.clear();
    lcd.setCursor(0, 0);
      lcd.print("NEW: " + String(wireLength, 1) + " mm");
      lcd.setCursor(0, 1);
      lcd.print("OLD: " + String(OldwireLength, 1) + " mm");
      delay(100);
    char key = keypad.getKey();
    if (key == '#') {
      // Handle '#' key press (confirmation)
      state = 4; // Go to the next state for wire quantity input
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.setCursor(0, 0);
      lcd.print("NEW: " + String(wireLength, 1) + " mm");
      lcd.setCursor(0, 1);
      lcd.print("OLD: " + String(OldwireLength, 1) + " mm");
      delay(100);
    } else if (key == '*') {
      // Handle '*' key press (back)
      state = 0;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("NEW: " + String(wireLength, 1) + " mm");
      lcd.setCursor(0, 1);
      lcd.print("OLD: " + String(OldwireLength, 1) + " mm");
      delay(100);
    } else if (key == 'D') {
      // Handle 'D' key press (clear input)
      wireLength = 0.0; // Remove the decimal part
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("NEW: " + String(wireLength, 1) + " mm");
      lcd.setCursor(0, 1);
      lcd.print("OLD: " + String(OldwireLength, 1) + " mm");
      delay(100);
    } else if (isdigit(key) || key == '.') {
      // Accumulate length input
      if (key == '.' && !String(wireLength).endsWith(".")) {
        // Check if the decimal point is pressed and not already present in the input
        // Set a flag to indicate that the next digits will be decimal digits
        wireLength = wireLength * 0.1; // Move the decimal point to the right
      } else if (isdigit(key)) {
        // Update the wire length based on the key pressed
        wireLength = wireLength * 10 + (key - '0');
      }
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("NEW: " + String(wireLength, 1) + " mm");
      lcd.setCursor(0, 1);
      lcd.print("OLD: " + String(OldwireLength, 1) + " mm");
      delay(100);
    }
  }
}



void confirm(){
  if (wireLength != 0  &&  wireQuantity != 0)
  {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print((String)wireLength + "mm x " + (String)wireQuantity + "pcs");
  lcd.setCursor(0, 3);
  lcd.print("<PISW");
  lcd.setCursor(11, 3);
  lcd.print("EKKINHSH>");
  delay(100);
  state = 1;

}
}
void currentlyCutting(){
  lcd.clear();
  lcd.setCursor(0, 0);
  long jobLength = wireLength * wireQuantity;
  long fullstepsToTake = jobLength / mmPerStep;
  long stepsToTake = wireLength / mmPerStep;
  long tempsteps = fullstepsToTake - wireLength;
  lcd.print((String)wireLength +"mm    ");
  MCutOn();
  delay(500);
  lcd.setCursor(0, 1);
  lcd.print("Moving...");
  stepperX.runToNewPosition(stepsToTake);
  MCutOff();
  OldwireLength= wireLength;
  wireLength=0;
  
  state = 0;
}

void MposOn()
  {
    lcd.clear();
    lcd.setCursor(12,3);
    lcd.print("MPOS ON");
    digitalWrite(Mpos,HIGH);
    delay(Maintimer);
  }

void MposOff()
  {
    lcd.clear();
    lcd.setCursor(12,3);
    lcd.print("MPOS OFF");
    digitalWrite(Mpos,LOW);
    delay(Maintimer);
  }


void MCutOn()
  {
    lcd.clear();
    lcd.setCursor(5,0);
    lcd.print("MCUT ON");
    digitalWrite(mcut,HIGH);
    delay(Maintimer);
    digitalWrite(mcut,LOW);
  }

void MCutOff()
  {
    lcd.clear();
    lcd.setCursor(5,1);
    lcd.print("MCUT OFF");
    digitalWrite(mcutoff,HIGH);
    delay(Maintimer);
    digitalWrite(mcutoff,LOW);
  }

void Kopi() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("DIADIKASIA KOPHS");
  digitalWrite(Cutter, HIGH);
  while (digitalRead(termatiko) == LOW) {
    }
  
  
  digitalWrite(Cutter, LOW);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("H KOPH TELEIWSE");
  delay(Maintimer);
}


//void currentlyCutting(){
//  lcd.setCursor(0, 0);
//  lcd.print((String)0 + "/" + (String)wireQuantity);
//  lcd.setCursor(0, 1);
//  lcd.print("YPOLOGISMOS XRONOU");
//  delay(1000);
//
//
//  //Anoigma meggenis kopis kleisimo meggenis topothetisis
//    digitalWrite(Mcut,LOW);
//    digitalWrite(McutOff,HIGH);
//    
//    lcd.clear();
//    lcd.setCursor(12, 2);
//    lcd.print("MCUT OFF");
//    delay(positiontime); // Edw mporei na mpei ena interlock asfaleias ean anoiksa oi meggenes kopis
//
//    digitalWrite(McutOff,LOW);
//    
//    digitalWrite(Mpos,HIGH);
//    lcd.setCursor(12,3);
//    lcd.print("MPOS ON");
//    delay(positiontime);
//       
//
//   //Metakinisi stin epithimiti thesi
//   
//    lcd.setCursor(3, 0);
//    lcd.print("METAKINHSH");
//    
//    int stepsToTake = (float)wireLength/mmPerStep;
//    for(int i = 0; i < wireQuantity; i++){
//
////      if(!digitalRead(Anixneusi))
////      {
////        break;
////      }
//
//      unsigned long timeForOneCycle = millis();
//      digitalWrite(dirPin,HIGH);
//      for(int x = 0; x < stepsToTake; x++) {
//        digitalWrite(stepPin,HIGH); 
//        delayMicroseconds(steptime); 
//        digitalWrite(stepPin,LOW); 
//        delayMicroseconds(steptime); 
//      }
//    // Oloklirwsi metakinhshs
//  
//    lcd.setCursor(3, 0);
//    lcd.print("H METAKINHSH");
//    lcd.setCursor(3, 1);
//    lcd.print("OLOKLIROTHIKE");
//
//    delay(Maintimer); 
//    
//    lcd.clear();
//    lcd.setCursor(12,2);
//    lcd.print("MCUT ON");
//    digitalWrite(Mcut,HIGH);
//    digitalWrite(McutOff,LOW);
//    delayMicroseconds(positiontime); 
//
//    digitalWrite(Mcut,LOW); 
//    
//    lcd.setCursor(12,3);
//    lcd.print("MPOS OFF");
//    digitalWrite(Mpos,LOW);
//
//    delay(Maintimer); 
//
//     int stepsToTake = (float)wireLength/mmPerStep;
//       digitalWrite(dirPin,LOW);
//      for(int x = 0; x < stepsToTake; x++) {
//        digitalWrite(stepPin,HIGH); 
//        delayMicroseconds(steptime); 
//        digitalWrite(stepPin,LOW); 
//        delayMicroseconds(steptime); 
//      }  
//      
//    lcd.clear(); 
//    lcd.setCursor(0,0);
//    lcd.print("DIADIKASIA KOPHS");
//    
//        digitalWrite(Cutter,HIGH); 
//    
//    delay(cuttime);
//
//        digitalWrite(Cutter,LOW); 
//
//    lcd.clear(); 
//    lcd.setCursor(0,0);
//    lcd.print("H KOPH TELEIWSE");
//    
//    delay(Maintimer); 
//
//    lcd.setCursor(12, 2);
//    lcd.print("MCUT OFF");
//    digitalWrite(McutOff,HIGH);
//    digitalWrite(Mcut,LOW);
//    delayMicroseconds(positiontime);
//    
//    digitalWrite(McutOff,LOW);
//    
//    lcd.setCursor(12,3);
//    lcd.print("MPOS ON");
//    digitalWrite(Mpos,HIGH);
//    
//    delay(positiontime);
//    
//    
//    
//    lcd.clear();
//    lcd.setCursor(0, 2);
//    lcd.print((String)(i+1) + "/" + (String)wireQuantity);
//    lcd.setCursor(0, 3);
//
//    unsigned long timeRemaining = ((millis() - timeForOneCycle)*(wireQuantity - (i+1)))/1000;
//    lcd.print((String)timeRemaining + "s    ");
//    snippers.write(closedAngle);
//  }
//  wireLength = 0;
//  wireQuantity = 0;
//  state = 5;
//}



void finishedCutting(){
  lcd.clear();
  lcd.setCursor(7, 0);
  lcd.print("H KOPH ");
   lcd.setCursor(3, 1);
  lcd.print("OLOKLIROTHIKE");
  lcd.setCursor(13, 3);
  lcd.print("EMPROS>");
  delay(100);
}

void MaterialFinish(){
  lcd.clear();
  lcd.setCursor(7, 0);
  lcd.print("TO YLIKO ");
   lcd.setCursor(3, 1);
  lcd.print("TELEIWSE");
  lcd.setCursor(13, 3);
  lcd.print("EMPROS>");
  delay(100);
}


float changeValue(float currentValue){
  if (ButtonState == UP) {
    currentValue ++;
    incrementSpeed =1;
  }
  if (ButtonState == START) {
    currentValue = currentValue+0.1;
    incrementSpeed =10;
  }
  if (ButtonState == DOWN) {
    if(currentValue - incrementSpeed >= 0){
      currentValue -= incrementSpeed;
      incrementSpeed =1;
    }
    else{
      currentValue = 0;
    }
 
  }
  return currentValue;
}

void displayNavigation(){
  lcd.setCursor(0, 3);
  lcd.print("<PISW");
  lcd.setCursor(9, 3);
  lcd.print("EMPROS>");
 delay(50);
}