// =========================================
// MESIN TANCAP BULU SHUTTLECOCK - V2
// Versi OLED 128x64 (I2C 0.96") - Tanpa EEPROM
// =========================================

#include <AccelStepper.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED Display
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Stepper motor
#define DIR_A 11
#define STEP_A 12
#define DIR_B 8
#define STEP_B 9
AccelStepper motorA(AccelStepper::DRIVER, STEP_A, DIR_A);
AccelStepper motorB(AccelStepper::DRIVER, STEP_B, DIR_B);

// Pin input/output
#define START_BUTTON A1
#define RESET_BUTTON A2
#define POT_PIN A0
#define RELAY_PIN 10
#define BUZZER_PIN 7

// Microstepping A4988
#define MS1 5
#define MS2 6
#define MS3 4
#define STEPS_PER_REV 1600

// Variabel
bool running = false;
unsigned long lastDebounce = 0;
int kokCount = 0;

// Setup awal
void setup() {
  pinMode(START_BUTTON, INPUT_PULLUP);
  pinMode(RESET_BUTTON, INPUT_PULLUP);
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  digitalWrite(RELAY_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);
  digitalWrite(MS1, LOW);
  digitalWrite(MS2, HIGH);
  digitalWrite(MS3, LOW);

  motorA.setMaxSpeed(1000);
  motorA.setAcceleration(500);
  motorB.setMaxSpeed(8000);
  motorB.setAcceleration(800);

  // Inisialisasi OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    for (;;); // Gagal inisialisasi
  }
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.clearDisplay();
  showInitial(); // Tampilkan status awal
}

// Loop utama
void loop() {
  if (digitalRead(START_BUTTON) == LOW && millis() - lastDebounce > 300) {
    lastDebounce = millis();
    running = true;
    runSequence();
  }

  if (digitalRead(RESET_BUTTON) == LOW && millis() - lastDebounce > 300) {
    lastDebounce = millis();
    kokCount = 0;
    showStatus("Reset", kokCount);
    delay(1000);
    showInitial();
  }
}

// Prosedur utama mesin
void runSequence() {
  motorA.setCurrentPosition(0);
  motorB.setCurrentPosition(0);

  showStatus("Running", kokCount);
  digitalWrite(RELAY_PIN, HIGH);
  delay(500);

  struct Step {
    float angle;
    int stepB;
  };

  Step steps[] = {
    { 0, 1600 }, { 22.5, 3200 }, { 45, 3200 }, { 67.5, 3200 },
    { 90, 3200 }, { 112.5, 3200 }, { 135, 3200 }, { 157.5, 3200 },
    { 180, 3200 }, { 202.5, 3200 }, { 225, 3200 }, { 247.5, 3200 },
    { 270, 3200 }, { 292.5, 3200 }, { 315, 3200 }, { 337.5, 3200 },
    { 360, 1600 }
  };

  for (int i = 0; i < sizeof(steps) / sizeof(steps[0]); i++) {
    moveMotorA(steps[i].angle);
    moveMotorB(steps[i].stepB);
  }

  digitalWrite(RELAY_PIN, LOW);
  beep();

  kokCount++;
  showStatus("Done", kokCount);
}

// Fungsi motor A (rotasi)
void moveMotorA(float angle) {
  int steps = (angle / 360.0) * STEPS_PER_REV;
  motorA.moveTo(steps);
  while (motorA.distanceToGo() != 0) {
    updateSpeed();
    motorA.runSpeed();
  }
}

// Fungsi motor B (tancap)
void moveMotorB(int steps) {
  motorB.moveTo(motorB.currentPosition() + steps);
  while (motorB.distanceToGo() != 0) {
    updateSpeed();
    motorB.runSpeed();
  }
}

// Pembacaan potensiometer untuk kecepatan
void updateSpeed() {
  int potValue = analogRead(POT_PIN);
  int speed = map(potValue, 0, 1023, 1000, 8000);
  motorA.setSpeed(speed);
  motorB.setSpeed(speed);
}

// Fungsi buzzer
void beep() {
  for (int i = 0; i < 2; i++) {
    digitalWrite(BUZZER_PIN, HIGH);
    delay(200);
    digitalWrite(BUZZER_PIN, LOW);
    delay(200);
  }
}

/// Tampilkan status di OLED
void showStatus(String status, int count) {
  display.clearDisplay();

  // Baris status (ukuran normal)
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("Status: ");
  display.println(status);

  // Label "Jumlah Kok"
  display.setTextSize(1);
  display.setCursor(0, 20);
  display.println("Jumlah Kok:");

  // Angka jumlah kok besar
  display.setTextSize(2);  // Ukuran besar
  display.setCursor(0, 36);
  display.println(count);

  display.display();
}


// Tampilkan status awal
void showInitial() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("MESIN TANCAP BULU");
  display.setCursor(0, 20);
  display.println("Status: Ready");
  display.setCursor(0, 40);
  display.println("Tekan START...");
  display.display();
}