from machine import Pin
import time

# ================= STEPPER MOTOR =================
STEP = Pin(14, Pin.OUT)
DIR  = Pin(12, Pin.OUT)

# Configure full-step mode
MS1 = Pin(32, Pin.OUT)
MS2 = Pin(33, Pin.OUT)
MS3 = Pin(25, Pin.OUT)

MS1.value(0)
MS2.value(0)
MS3.value(0)


# ================= LCD =================
rs = Pin(22, Pin.OUT)
e  = Pin(23, Pin.OUT)

d4 = Pin(21, Pin.OUT)
d5 = Pin(13, Pin.OUT)
d6 = Pin(26, Pin.OUT)
d7 = Pin(27, Pin.OUT)


def pulse():
    e.value(1)
    time.sleep_us(1000)
    e.value(0)


def send_nibble(data):
    d4.value((data >> 0) & 1)
    d5.value((data >> 1) & 1)
    d6.value((data >> 2) & 1)
    d7.value((data >> 3) & 1)
    pulse()


def send_byte(data, mode):
    rs.value(mode)
    send_nibble(data >> 4)
    send_nibble(data & 0x0F)


def lcd_cmd(cmd):
    send_byte(cmd, 0)
    time.sleep_ms(2)


def lcd_data(data):
    send_byte(data, 1)
    time.sleep_ms(2)


def lcd_clear():
    lcd_cmd(0x01)


def lcd_init():
    time.sleep_ms(50)
    lcd_cmd(0x33)
    lcd_cmd(0x32)
    lcd_cmd(0x28)
    lcd_cmd(0x0C)
    lcd_cmd(0x06)
    lcd_cmd(0x01)


def lcd_print(text):
    for c in text:
        lcd_data(ord(c))


# ================= KEYPAD =================
rows = [Pin(i, Pin.OUT) for i in [19, 18, 5, 17]]
cols = [Pin(i, Pin.IN, Pin.PULL_UP) for i in [16, 4, 2, 15]]

keys = [
    ['1','2','3','A'],
    ['4','5','6','B'],
    ['7','8','9','C'],
    ['*','0','#','D']
]


def read_key():
    for i, r in enumerate(rows):
        r.value(0)

        for j, c in enumerate(cols):
            if c.value() == 0:
                r.value(1)
                return keys[i][j]

        r.value(1)
    return None


def wait_release():
    while read_key():
        time.sleep_ms(50)


# ================= ELEVATOR LOGIC =================
current_floor = 1
STEPS_PER_FLOOR = 200

# Track current motor position in steps
position_steps = 0


# ================= MOTOR =================
def step_motor(direction, steps):

    DIR.value(direction)

    for _ in range(steps):

        STEP.value(1)
        time.sleep_us(1200)

        STEP.value(0)
        time.sleep_us(1200)


# ================= DISPLAY UPDATE =================
def show_floor():

    lcd_clear()
    lcd_print("Floor: ")
    lcd_print(str(current_floor))

    print("DISPLAY POSITION =", (current_floor - 1) * 200)


# ================= MOVE FUNCTION =================
def go_to_floor(target):

    global current_floor, position_steps

    if target == current_floor:
        return

    diff = target - current_floor
    steps = abs(diff) * STEPS_PER_FLOOR

    if diff > 0:
        step_motor(1, steps)
        position_steps += steps
    else:
        step_motor(0, steps)
        position_steps -= steps

    current_floor = target

    # Display position using floor-based step calculation
    print("RAW STEPS:", position_steps)
    print("FIXED OUTPUT:", (current_floor - 1) * 200)

    show_floor()


# ================= START =================
lcd_init()
show_floor()

print("Elevator Ready")


# ================= MAIN LOOP =================
while True:

    key = read_key()

    if key:

        print("Pressed:", key)

        if key in ['1','2','3','4','5','6']:
            go_to_floor(int(key))

        wait_release()

    time.sleep_ms(50)