#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <Wire.h>

LiquidCrystal_I2C lcd(0x27,20,4);
byte upChar[] = {   
  B00000,
  B00100,
  B01010,
  B10001,
  B00000,
  B00000,
  B00000,
  B00000
};
byte downChar[] = {
  B00000,
  B00000,
  B00000,
  B00000,
  B10001,
  B01010,
  B00100,
  B00000
  };

#define stepPin 32
#define dirPin 33

Servo snippers;
Servo location;

#define servo1 25   //pin Servo1,potong
#define servo2 26  //pin Servo2,kupas
#define openAngle 0  //Derajat servo1 potong
#define closedAngle 180 //Derajat servo1 kembali
#define cut 70    //Derajat servo2 potong
#define strip 118 //Derajat servo2 kupas

//timing -------------------------
#define cutdelay 1000 //Defines the time the cut servo needs to reach positions
#define stepdelay 150 //Defines paustime between step pulses = speed of feed, lower = faster

//input tombol---------------------------
#define leftButton 12
#define rightButton 13
#define upButton 35
#define downButton 27
#define fButton 14

//user settings -------------------
unsigned int wireLength = 14;
unsigned int stripLength = 4;
unsigned int wireQuantity = 5;

//system settings --------------
int state = 0;
int incrementSpeed = 1;
int previousWireLength = 0;
int previousStripLength = 0;
int previousWireQuantity = 0;
float mmPerStep = 0.010158;   //Ratio mm feeded per step of motor 0.020458


void setup() {

  // Initialize serial communication
  Serial.begin(9600);

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  //lcd.createChar(0, upChar);
  //lcd.createChar(1, downChar);
  pinMode(upButton, INPUT_PULLUP);
  pinMode(downButton, INPUT_PULLUP);
  pinMode(leftButton, INPUT_PULLUP);
  pinMode(rightButton, INPUT_PULLUP);
  pinMode(fButton, INPUT_PULLUP);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  snippers.attach(servo1);
  location.attach(servo2);
  snippers.write(openAngle);
  delay(cutdelay);
  location.write(strip);
  delay(300);
}

void loop() {
  if (!digitalRead(rightButton)) {
    if (state == 6) {
      state = 0;
    }
    else {
      state += 1;
    }
    delay(200);
    lcd.clear();
  }
  if (!digitalRead(leftButton) && state > 0 && state < 5) {
    state -= 1;
    delay(200);
    lcd.clear();
  }
  if (!digitalRead(upButton) && state == 0) {
     feedFwd();
  }
  if (!digitalRead(downButton)&& state == 0) {
    feedRwd();
  }
  if (!digitalRead(fButton)&& state == 0) {
    cutting();
  }

  switch (state) {
    case 0:
      homeScreen();
      break;
    case 1:
      chooseWireLength();
      break;
    case 2:
      chooseStripLength();
      break;
    case 3:
      chooseWireQuantity();
      break;
    case 4:
      confirm();
      break;
    case 5:
      currentlyCutting();
      break;
    case 6:
      finishedCutting();
      break;
  }

}


void homeScreen() {
  lcd.setCursor(0, 0);
  lcd.print("  Wire Cut & Strip");
  lcd.setCursor(0, 1);
  lcd.write(byte(0));
  lcd.print(" Feed Forward");
  lcd.setCursor(0, 2);
  lcd.write(byte(1));
  lcd.print(" Feed Rewerse");
  lcd.setCursor(0, 3);
  lcd.print("* Cut          NEXT>");
  delay(100);
  
  }



void chooseWireLength() {
  wireLength = changeValue(wireLength);

  //clear LCD if required
  if (previousWireLength != wireLength) {
    lcd.clear();
    previousWireLength = wireLength;
  }

  //Display information on LCD
  lcd.setCursor(0, 0);
  lcd.print("TOTAL:" + (String)wireLength + "mm");
  lcd.setCursor(0, 1);
  lcd.print("Step x " + (String)incrementSpeed + "mm  ");
  lcd.setCursor(0, 2);
  lcd.print("* to toggle 1/10");
  displayNavigation();
}

void chooseStripLength() {
  stripLength = changeValue(stripLength);

  //clear LCD if required
  if (previousStripLength != stripLength) {
    lcd.clear();
    previousStripLength = stripLength;
  }

  //Display information on LCD
  lcd.setCursor(0, 0);
  lcd.print("STRIP:" + (String)stripLength + "mm");
  lcd.setCursor(0, 1);
  lcd.print("Step x " + (String)incrementSpeed + "mm  ");
  lcd.setCursor(0, 2);
  lcd.print("* to toggle 1/10");
  displayNavigation();
}

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("QUANTITY:" + (String)wireQuantity);
  lcd.setCursor(0, 1);
  lcd.print("Step x " + (String)incrementSpeed + "mm  ");
  lcd.setCursor(0, 2);
  lcd.print("* to toggle 1/10");
  displayNavigation();
}

void confirm() {
  lcd.setCursor(0, 0);
  lcd.print((String)wireLength + "mm x " + (String)wireQuantity + "pcs");
  lcd.setCursor(0, 3);
  lcd.print("<BACK");
  lcd.setCursor(14, 3);
  lcd.print("START>");
  delay(100);
}

void currentlyCutting() {

  if (stripLength == 0)   //decide if stripping or not shall ocure
  { //Start with no stripping
    lcd.setCursor(0, 0);
    lcd.print((String)0 + "/" + (String)wireQuantity);
    lcd.setCursor(0, 3);
    lcd.print("???s");
    int stepsToTake = (int)wireLength / mmPerStep;
    for (int i = 0; i < wireQuantity; i++) {
      unsigned long timeForOneCycle = millis();
      digitalWrite(dirPin, HIGH);
      for (int x = 0; x < stepsToTake; x++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepdelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepdelay);
      }

      lcd.setCursor(0, 0);
      lcd.print((String)(i + 1) + "/" + (String)wireQuantity);

      location.write(cut);
      delay(300);
      snippers.write(closedAngle);
      delay(cutdelay);
      snippers.write(openAngle);
      delay(cutdelay);
      location.write(strip);
      delay(300);


      lcd.setCursor(0, 3);

      unsigned long timeRemaining = ((millis() - timeForOneCycle) * (wireQuantity - (i + 1))) / 1000;
      lcd.print((String)timeRemaining + "s    ");

    }
    //wireLength = 14;
    //wireQuantity = 5;
    state = 6;
  }

  else
  { //Start with stripping operation

    lcd.setCursor(0, 0);
    lcd.print((String)0 + "/" + (String)wireQuantity);
    lcd.setCursor(0, 3);
    lcd.print("???s");
    //Calculate Steps to take
    int stripStepsToTake = (int)stripLength / mmPerStep;
    int totalStepsToTake = (int)wireLength / mmPerStep;
    int twoStripsSteps = stripStepsToTake * 2;
    int stepsToTake = totalStepsToTake - twoStripsSteps;



    for (int i = 0; i < wireQuantity; i++) {
      unsigned long timeForOneCycle = millis();
      digitalWrite(dirPin, HIGH);

      //Feed first strip length
      for (int x = 0; x < stripStepsToTake; x++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepdelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepdelay);
      }
      //First Strip cut
      snippers.write(closedAngle);
      delay(cutdelay);
      snippers.write(openAngle);
      delay(cutdelay);

      //Feed middle wire
      for (int x = 0; x < stepsToTake; x++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepdelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepdelay);
      }
      //Second strip cut
      snippers.write(closedAngle);
      delay(cutdelay);
      snippers.write(openAngle);
      delay(cutdelay);

      //Feed end strip lenght
      for (int x = 0; x < stripStepsToTake; x++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepdelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepdelay);
      }
      //Finaly cut wire

      lcd.setCursor(0, 0);
      lcd.print((String)(i + 1) + "/" + (String)wireQuantity);

      location.write(cut);
      delay(300);
      snippers.write(closedAngle);
      delay(cutdelay);
      snippers.write(openAngle);
      delay(cutdelay);
      location.write(strip);
      delay(300);


      lcd.setCursor(0, 3);

      unsigned long timeRemaining = ((millis() - timeForOneCycle) * (wireQuantity - (i + 1))) / 1000;
      lcd.print((String)timeRemaining + "s    ");

    }
    //wireLength = 14;
    //wireQuantity = 5;
    state = 6;
  }
}

void finishedCutting() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("CUTTING COMPLETE");
  lcd.setCursor(15, 3);
  lcd.print("NEXT>");
  delay(100);
}


int changeValue(int currentValue) {
  if (!digitalRead(upButton)) {
    delay(100);
    currentValue += incrementSpeed;

  }
  if (!digitalRead(downButton)) {
    if (currentValue - incrementSpeed >= 0) {
      delay(100);
      currentValue -= incrementSpeed;

    }
    else {
      currentValue = 0;
    }
  }
  if (!digitalRead(fButton)) {
        if (incrementSpeed == 1) {
          delay (100);
          incrementSpeed = 10;
        }
        else {
          incrementSpeed = 1;
          delay (100);
        }
        }
  
  return currentValue;
}

void displayNavigation() {
  lcd.setCursor(0, 3);
  lcd.print("<BACK");
  lcd.setCursor(15, 3);
  lcd.print("NEXT>");
  delay(100);
}

void feedFwd() {       //Feed forward
for (int x = 0; x < 50; x++) {
  digitalWrite(dirPin, HIGH);
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(stepdelay);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(stepdelay);
}
}

void feedRwd() {       //Feed backward
for (int x = 0; x < 50; x++) {
  digitalWrite(dirPin, LOW);
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(stepdelay);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(stepdelay);
}
}

void cutting() {    //Cutting operation
      location.write(cut);
      delay(300);
      snippers.write(closedAngle);
      delay(cutdelay);
      snippers.write(openAngle);
      delay(cutdelay);
      location.write(strip);
      delay(300);
}