#include <LiquidCrystal.h>
#include <Servo.h>
LiquidCrystal lcd(12,11,5,4,3,2);
byte upChar[] = {   
  B00000,
  B00100,
  B01010,
  B10001,
  B00000,
  B00000,
  B00000,
  B00000
};
byte downChar[] = {
  B00000,
  B00000,
  B00000,
  B00000,
  B10001,
  B01010,
  B00100,
  B00000
};
#define stepPin 7
#define dirPin 8

Servo snippers;
Servo location;

#define servo1 9   //pin Servo1,potong
#define servo2 10  //pin Servo2,kupas
#define openAngle 20  //Derajat servo1 potong
#define closedAngle 130 //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 A0
#define rightButton A4
#define upButton A3
#define downButton A2
#define fButton A1

//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;   //Rasio mm yang diumpankan per langkah motor 0,020458
int cm = 10;

void setup() {
  Serial.begin(9600);
  lcd.begin(20, 4);
  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(12, 3);
  lcd.print("LANJUT>>");
  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("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);

  //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("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)   //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 / 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 / mmPerStep;
    int twoStripsSteps = stripStepsToTake * 2;
    int stepsToTake = totalStepsToTake - twoStripsSteps;



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

      //Beri makan panjang strip pertama
      for (int x = 0; x < stripStepsToTake; x++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepdelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepdelay);
      }
      //Potongan Strip Pertama
      snippers.write(closedAngle);
      delay(cutdelay);
      snippers.write(openAngle);
      delay(cutdelay);

      //Umpankan kabel tengah
      for (int x = 0; x < stepsToTake; x++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepdelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepdelay);
      }
      //Potongan strip kedua
      snippers.write(closedAngle);
      delay(cutdelay);
      snippers.write(openAngle);
      delay(cutdelay);

      //Panjang strip ujung umpan
      for (int x = 0; x < stripStepsToTake; x++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepdelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepdelay);
      }
      //Kawat potong terakhir

      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 = 2;
        }
        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() {       //Umpan ke depan
for (int x = 0; x < 50; x++) {
  digitalWrite(dirPin, HIGH);
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(stepdelay);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(stepdelay);
}
}

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

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