# ==========================================================
#  Camera Slider - Position Control
#  ESP32 + Stepper (A4988) + Rotary Encoder + I2C LCD 16x2
#  MicroPython - Wokwi Simulation
# ==========================================================

from machine import Pin, I2C
import time
from i2c_lcd import I2cLcd

# ---------------- Stepper Driver Pins ----------------
step_pin   = Pin(14, Pin.OUT)
dir_pin    = Pin(12, Pin.OUT)
enable_pin = Pin(13, Pin.OUT)

ms1 = Pin(26, Pin.OUT)
ms2 = Pin(27, Pin.OUT)
ms3 = Pin(19, Pin.OUT)   

enable_pin.value(0)   # Enable driver (LOW = Enabled)

# Microstepping: Full step (MS1=MS2=MS3=0)
ms1.value(0)
ms2.value(0)
ms3.value(0)

# ---------------- Encoder Pins ----------------
clk = Pin(32, Pin.IN, Pin.PULL_UP)
dt  = Pin(33, Pin.IN, Pin.PULL_UP)
sw  = Pin(25, Pin.IN, Pin.PULL_UP)

# ---------------- I2C LCD Setup ----------------
i2c = I2C(0, scl=Pin(22), sda=Pin(21))
devices = i2c.scan()
print("I2C devices:", devices)

if devices:
    lcd = I2cLcd(i2c, devices[0], 2, 16)  # 16x2 LCD
else:
    lcd = None
    print("No LCD found!")

# ---------------- System Parameters ----------------
STEPS_PER_CM = 20
STEP_DELAY = 0.001
MAX_POSITION_CM = 100
MIN_POSITION_CM = 0

# ---------------- Global Variables ----------------
target_position_cm = 0
current_position_cm = 0
last_clk_state = clk.value()
position_changed = False
home_pressed = False


# ==========================================================
# Encoder Interrupt Handler - Sets Target Position
# ==========================================================
def encoder_callback(pin):
    global target_position_cm, last_clk_state, position_changed

    clk_state = clk.value()
    if clk_state != last_clk_state and clk_state == 0:
        dt_state = dt.value()

        # check clockwise
        if dt_state != clk_state :
            target_position_cm +=1
        else:
            #counter clockwise
            target_position_cm -=1
        # clamp
        # -50
        # t= max(0, min(100, t))
        target_position_cm = max(MIN_POSITION_CM, min(MAX_POSITION_CM, target_position_cm))
        # if target_position_cm > MAX_POSITION_CM:
        #     target_position_cm = MAX_POSITION_CM
        # elif target_position_cm < MIN_POSITION_CM:
        #     target_position_cm = MIN_POSITION_CM
        position_changed = True
    last_clk_state = clk_state


# ==========================================================
# Button Interrupt Handler - Set Home Position
# ==========================================================
def button_callback(pin):
    global home_pressed
    time.sleep_ms(50)
    if sw.value() == 0:
        home_pressed = True


clk.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=encoder_callback)
sw.irq(trigger=Pin.IRQ_FALLING, handler=button_callback)


# ==========================================================
# Function: Move Stepper Motor
# ==========================================================
def move_stepper(steps, direction):
    dir_pin.value(direction)
    for _ in range(steps):
        step_pin.value(1)
        time.sleep(STEP_DELAY)
        step_pin.value(0)
        time.sleep(STEP_DELAY)


# ==========================================================
# Function: Move Camera to Target Position
# ==========================================================
def move_to_position(target_cm):
    global current_position_cm

    diff_cm = target_cm - current_position_cm
    if diff_cm == 0:
        return

    direction = 1 if diff_cm > 0 else 0
    steps_needed = abs(diff_cm) * STEPS_PER_CM

    move_stepper(steps_needed, direction)
    current_position_cm = target_cm


# ==========================================================
# Function: Update LCD Display
# ==========================================================
def update_display():
    if lcd is None:
        print("Camera Position:", current_position_cm, "cm")
        return

    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr("Camera Slider")
    lcd.move_to(0, 1)
    lcd.putstr("Pos: " + str(current_position_cm) + " cm")


# ==========================================================
# Function: Set Home Position
# ==========================================================
def set_home():
    global target_position_cm, current_position_cm

    print("Setting Home Position...")
    if lcd:
        lcd.clear()
        lcd.move_to(0, 0)
        lcd.putstr("Setting Home...")
    time.sleep(0.5)

    target_position_cm = 0
    current_position_cm = 0
    move_to_position(0)


# ---------------- Initial Display ----------------
update_display()
print("Camera Slider System Ready")

# ==========================================================
# Main Loop
# ==========================================================
while True:
    if position_changed:
        move_to_position(target_position_cm)
        update_display()
        print("Target Position: ", target_position_cm)
        position_changed = False

    if home_pressed:
        set_home()
        update_display()
        home_pressed = False
        print("Back Home")