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

LiquidCrystal_I2C lcd(0x27,20,4);

//stepper-----------
#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 500 //Menentukan waktu yang dibutuhkan servo potong untuk mencapai posisi
#define stepdelay 400 //Mendefinisikan waktu jeda antara pulsa langkah = kecepatan umpan, lebih rendah = lebih cepat
//input tombol---------------------------
#define leftButton 14
#define rightButton 16
#define upButton 18
#define downButton 17
#define fButton 19

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

//system settings --------------
int cm = 10;
int state = 0;
int incrementSpeed = 1;
int previousWireLength = 0;
int previousStripLength = 0;
int previousWireQuantity = 0;
float mmPerStep = 0.010158;   //Rasio mm yang diumpankan per langkah motor 0,020458


void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  lcd.clear();
  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("               NEXT>");
  delay(100);
  
  }



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

   //Hapus LCD jika diperlukan
  if (previousWireLength != wireLength) {
    lcd.clear();
    previousWireLength = wireLength;
  }

  //Menampilkan informasi pada LCD
  lcd.setCursor(0, 0);
  lcd.print("PANJANG : " + (String)wireLength + " cm");
  lcd.setCursor(0, 1);
  lcd.print("Step x " + (String)incrementSpeed + " cm  ");
  lcd.setCursor(0, 2);
  lcd.print("* to toggle 1/10");
  displayNavigation();
}

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

  //Hapus LCD jika diperlukan
  if (previousStripLength != stripLength) {
    lcd.clear();
    previousStripLength = stripLength;
  }

   //Menampilkan informasi pada 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);

  //Hapus LCD jika diperlukan
  if (previousWireQuantity != wireQuantity) {
    lcd.clear();
    previousWireQuantity = wireQuantity;
  }

   //Menampilkan informasi pada LCD
  lcd.setCursor(0, 0);
  lcd.print("JUMLAH : " + (String)wireQuantity + " pcs");
  lcd.setCursor(0, 1);
  lcd.print("Step x " + (String)incrementSpeed + " pcs ");
  lcd.setCursor(0, 2);
  lcd.print("* to toggle 1/10");
  displayNavigation();
}

void confirm() {
  lcd.setCursor(0, 0);
  lcd.print((String)wireLength + " cm 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)   //memutuskan apakah pengupasan akan dilakukan atau tidak
  { //Mulailah tanpa pengupasan
    lcd.setCursor(0, 0);
    lcd.print((String)0 + "/" + (String)wireQuantity);
    lcd.setCursor(0, 3);
    lcd.print("???s");
    int stepsToTake = (int)wireLength * (int)cm / 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
  {  //Mulailah dengan operasi pengupasan

    lcd.setCursor(0, 0);
    lcd.print((String)0 + "/" + (String)wireQuantity);
    lcd.setCursor(0, 3);
    lcd.print("??? s");
    //Hitung Langkah yang harus diambil
    int stripStepsToTake = (int)stripLength / mmPerStep;
    int totalStepsToTake = (int)wireLength * (int)cm / 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("POTONG SELESAI......");
  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);
}