/*
  Điều khiển servo bằng biến trở + 2 nút + LCD I2C (Nucleo C031C6)
  - Biến trở : chỉnh tốc độ quét servo
  - Button 1 (D2, nút xanh): bấm 1 lần bật, bấm lần nữa tắt
  - Button 2 (D4): 
      + Servo đang TẮT  -> giữ thì quay, thả ra thì dừng
      + Servo đang BẬT  -> giữ thì DỪNG, thả ra thì chạy tiếp
  - LCD: dòng 1 = Servo ON/OFF, dòng 2 = Speed %

  Nối dây:
  LCD    : GND->GND, VCC->5V, SDA->D14, SCL->D15
  Button : 1 chân -> D2 (hoặc D4), chân kia -> GND
  Servo  : GND->GND, V+->5V, Signal->D3
  Biến trở: GND->GND, VCC->3V3 (không dùng 5V), SIG->A0
*/

#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

#define BTN_TOGGLE  D2     // Button 1: bấm để bật/tắt
#define BTN_HOLD    D4     // Button 2: giữ
#define SERVO_PIN   D3     // Tín hiệu servo
#define POT_PIN     A0     // Biến trở

LiquidCrystal_I2C lcd(0x27, 16, 2);   // Địa chỉ LCD 0x27, 16 cột, 2 hàng
Servo myServo;

// --- Button 1 (toggle) ---
bool toggleState = false;             // false = tắt, true = bật
bool lastToggleRead = HIGH;           // Giá trị đọc vòng trước
bool stableToggle = HIGH;             // Trạng thái đã ổn định (sau chống dội)
unsigned long lastDebounce = 0;
const unsigned long DEBOUNCE_MS = 30; // Chống dội 30ms

// --- Servo ---
int angle = 0;                        // Góc hiện tại 0..180
int dir = 1;                          // +1 tăng góc, -1 giảm góc
unsigned long lastStep = 0;           // Thời điểm nhích bước gần nhất

// --- LCD: chỉ ghi khi giá trị thay đổi ---
bool lastRunning = false;
int  lastSpeedPct = -1;
bool firstDraw = true;

void updateLCD(bool running, int speedPct) {
  if (firstDraw || running != lastRunning) {
    lcd.setCursor(0, 0);
    lcd.print(running ? "Servo: ON       " : "Servo: OFF      ");  // dấu cách cuối để xóa ký tự cũ
    lastRunning = running;
  }
  if (firstDraw || speedPct != lastSpeedPct) {
    lcd.setCursor(0, 1);
    lcd.print("Speed: ");
    lcd.print(speedPct);
    lcd.print("%   ");
    lastSpeedPct = speedPct;
  }
  firstDraw = false;
}

void setup() {
  pinMode(BTN_TOGGLE, INPUT_PULLUP);  // Nhấn nút -> LOW, không cần điện trở ngoài
  pinMode(BTN_HOLD, INPUT_PULLUP);
  analogReadResolution(10);           // analogRead trả về 0..1023

  myServo.attach(SERVO_PIN);
  myServo.write(angle);               // Servo về 0° lúc bắt đầu

  lcd.init();
  lcd.backlight();
  lcd.clear();
}

void loop() {
  // ===== Button 1: bấm để bật/tắt (có chống dội) =====
  bool readToggle = digitalRead(BTN_TOGGLE);
  if (readToggle != lastToggleRead) {
    lastDebounce = millis();          // Nút vừa đổi -> đếm lại 30ms
  }
  if (millis() - lastDebounce > DEBOUNCE_MS && readToggle != stableToggle) {
    stableToggle = readToggle;
    if (stableToggle == LOW) {        // Chỉ xử lý lúc vừa nhấn xuống
      toggleState = !toggleState;     // Đảo bật <-> tắt
    }
  }
  lastToggleRead = readToggle;

  // ===== Button 2: đang giữ hay không =====
  bool holdState = (digitalRead(BTN_HOLD) == LOW);

  // ===== QUYẾT ĐỊNH SERVO CÓ CHẠY KHÔNG (XOR: khác nhau thì chạy) =====
  //  toggle tắt + không giữ -> DỪNG
  //  toggle bật + không giữ -> CHẠY
  //  toggle tắt + giữ       -> CHẠY
  //  toggle bật + giữ       -> DỪNG   (chức năng mới)
  bool running = (toggleState != holdState);

  // ===== Biến trở -> tốc độ =====
  int pot = analogRead(POT_PIN);                          // 0..1023
  int speedPct = map(pot, 0, 1023, 0, 100);               // % hiển thị LCD
  unsigned long stepInterval = map(pot, 0, 1023, 20, 2);  // ms/bước: nhỏ = nhanh (chỉnh 20 và 2 để đổi tốc độ)

  // ===== Quét servo (không dùng delay) =====
  if (running && millis() - lastStep >= stepInterval) {
    lastStep = millis();
    angle += dir;                                   // Muốn nhanh hơn: angle += dir * 3;
    if (angle >= 180) { angle = 180; dir = -1; }    // Chạm 180° -> quay ngược
    if (angle <= 0)   { angle = 0;   dir = 1;  }    // Chạm 0°   -> đi lên
    myServo.write(angle);
  }

  // ===== LCD =====
  updateLCD(running, speedPct);
}