//SPECIFIC FOR 1.8 DEG STEPPER MOTOR//
#define MICROSTEP 1
#define SCREW_PITCH 2

#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Servo.h>

//PIN
const int STEP_X = 5, DIR_X = 6, STEP_COIL = 7, DIR_COIL = 8;  //PIN STEPPER MOTOR
const int buzzer = 10;
const int endStop_left = 11, endStop_right = 12;
const int servoPin = 9, potServo = A6;

float wire_diameter, solenoid_length;
int max_loop,n_loop;
int delta_x_step, max_loop_x;
float deg_per_step, x_per_step;
bool systemStart = false; 
bool startWinding = true;
bool dirX = false;

//VARIABLE KEYPAD
const int ROWS = 4, COLS = 3;
char keys[ROWS][COLS] = {
  { '1', '2', '3' },
  { '4', '5', '6'},
  { '7', '8', '9'},
  { '*', '0', '#'}
};
byte rowPins[ROWS] = { A0,A1,A2,A3};
byte colPins[COLS] = { 2,3,4};

LiquidCrystal_I2C lcd(0x27, 20, 4);
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);
Servo feederServo;

void setup() {
  Serial.begin(9600);
  pinMode(STEP_X, OUTPUT);
  pinMode(DIR_X, OUTPUT);
  pinMode(STEP_COIL, OUTPUT);
  pinMode(DIR_COIL, OUTPUT);
  pinMode(servoPin, OUTPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(endStop_left, INPUT_PULLUP);
  pinMode(endStop_right, INPUT_PULLUP);

  //INIT SERVO
  feederServo.attach(9);

  //INIT LCD
  lcd.init();
  lcd.display();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("-COIL WINDING-");
  lcd.setCursor(4, 2);
  lcd.print("SINGULARITAS LAB");
  delay(1000);  //delay sebelumnya 1000
  lcd.clear();

  //CALCULATE BASED ON CONFIG
  x_per_step = SCREW_PITCH / 200.0;
}

void loop() {
  char key = keypad.getKey();
  if (!systemStart) {
    if (key == '1') {
      //CALCULATE BASED ON INPUT
      setupConfig();
      delta_x_step = (float)wire_diameter / x_per_step;
      Serial.println(delta_x_step);
      systemStart = true;
    } else {
      //JIKA BELUM ADA TOMBOL YANG DITEKAN MAKA DISPLAY PILIHAN MENU
      lcd.setCursor(2, 0);
      lcd.print("PRESS 1 TO START");
    }
  } else {
    //PROGRAM UTAMA
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print(n_loop);
    if (digitalRead(endStop_left) == 0) 
      dirX = false;
    else if(digitalRead(endStop_right) == 0)
      dirX = true;
    if (n_loop < max_loop) {
      if (startWinding)
        windCoil();
      else
        spinMotorX(dirX, delta_x_step);
    } else {
      delay(500);
      lcd.clear();
      lcd.setCursor(5, 1);
      lcd.print("DONE");
      delay(1000);
      lcd.clear();
      n_loop = 0;
      systemStart = false;
    }
    //STOP WINDING MACHINE, BACK TO MENU
    if (key == '#') {
      lcd.clear();
      systemStart = false;
    }
  }
}

void windCoil() {
  for (int i = 0; i < 200; i++) {
    digitalWrite(STEP_COIL, HIGH);
    delayMicroseconds(1250);
    digitalWrite(STEP_COIL, LOW);
    delayMicroseconds(1250);
  }
  startWinding = false;
  n_loop += 1;
}

void spinMotorX(bool direction, int delta_x_step) {
  digitalWrite(DIR_X, direction);
  for (int i = 0; i < delta_x_step; i++) {
    digitalWrite(STEP_X, HIGH);
    delayMicroseconds(1250);
    digitalWrite(STEP_X, LOW);
    delayMicroseconds(1250);
  }
  startWinding = true;
}

void setupConfig() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.println("PARAMETER");
  lcd.setCursor(0, 1);
  lcd.print("Wire  (mm):0.");
  lcd.setCursor(0, 2);
  lcd.print("Loop      :");
  wire_diameter = ("0." + getKeypadInput(13, 1)).toFloat();
  max_loop = getKeypadInput(11, 2).toInt();
  systemStart = true;
}

// FUNGSI UNTUK MENANGANI INPUT KEYBOARD
String getKeypadInput(int col, int row) {
  lcd.setCursor(col, row);
  String input = "";
  char key;
  while (true) {
    key = keypad.getKey();
    if (key == '#') {
      digitalWrite(buzzer, HIGH);
      delay(50);
      digitalWrite(buzzer, LOW);
      return input;  // Confirm input
    }else if(key == '*'){

    }else if (key) {
      input += key;
      lcd.print(key);
    }
  }
}