use esp_idf_hal::prelude::*;
use esp_idf_hal::gpio::*;
use esp_idf_hal::i2c::{I2cConfig, I2cDriver};
use esp_idf_hal::delay::FreeRtos;
use ssd1306::{prelude::*, Builder, I2CDIBuilder};
use log::info;
use anyhow::Result;

// Konfigurasi pin untuk komponen
const STEP_PIN: u8 = 18;   // STEP untuk A4988
const DIR_PIN: u8 = 19;    // DIRECTION untuk A4988
const SERVO_PIN: u8 = 21;  // Servo (Pemotong)
const SOLENOID_PIN: u8 = 5; // Solenoid Penanggal Penebat

fn main() -> Result<()> {
    esp_idf_sys::link_patches();
    
    // Ambil periferal ESP32
    let peripherals = Peripherals::take().unwrap();
    let pins = peripherals.pins;

    // ✅ Inisialisasi OLED Display (SSD1306)
    let i2c = I2cDriver::new(
        peripherals.i2c0,
        pins.gpio22, // SCL
        pins.gpio21, // SDA
        &I2cConfig::new().baudrate(400.kHz().into()),
    )?;

    let interface = I2CDIBuilder::new().init(i2c);
    let mut display = Builder::new().connect(interface).into_terminal_mode();
    display.init().unwrap();
    display.clear().unwrap();
    display.write_str("MESIN SIAP!").unwrap();

    // ✅ Inisialisasi Stepper Motor (A4988)
    let mut step_pin = PinDriver::output(pins.gpio18)?;
    let mut dir_pin = PinDriver::output(pins.gpio19)?;
    
    // ✅ Inisialisasi Servo Motor (Pemotong)
    let mut servo_pin = PinDriver::output(pins.gpio21)?;
    
    // ✅ Inisialisasi Solenoid Penanggal Penebat
    let mut solenoid_pin = PinDriver::output(pins.gpio5)?;
    
    info!("Sistem mesin pemotong & penanggal wayar siap!");

    // 🔄 Loop operasi mesin
    loop {
        // 1️⃣ ✅ Stepper menarik wayar
        dir_pin.set_high()?;
        for _ in 0..200 {  // Putaran stepper (ubah ikut panjang wayar)
            step_pin.set_high()?;
            FreeRtos::delay_ms(1);
            step_pin.set_low()?;
            FreeRtos::delay_ms(1);
        }

        // 2️⃣ ✅ Servo menggerakkan pemotong
        servo_pin.set_high()?;
        FreeRtos::delay_ms(500); // Tunggu 0.5s
        servo_pin.set_low()?;
        FreeRtos::delay_ms(500);

        // 3️⃣ ✅ Solenoid menarik penebat wayar
        solenoid_pin.set_high()?;
        FreeRtos::delay_ms(1000); // Tunggu 1s
        solenoid_pin.set_low()?;
        
        // Paparkan status pada OLED
        display.clear().unwrap();
        display.write_str("Potong & Tanggal!").unwrap();
        
        FreeRtos::delay_ms(2000); // Tunggu sebelum ulang proses
    }
}