from machine import Pin, I2C, PWM
import time
import struct


# =====================================================
# STEPPER MOTOR
# =====================================================

STEP = Pin(14, Pin.OUT)
DIR = Pin(12, Pin.OUT)
ENABLE = Pin(13, Pin.OUT)

ENABLE.value(0)


# Full Step Mode
MS1 = Pin(26, Pin.OUT)
MS2 = Pin(27, Pin.OUT)
MS3 = Pin(25, Pin.OUT)

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



# =====================================================
# ENCODER
# =====================================================

clk = Pin(34, Pin.IN)
dt = Pin(32, Pin.IN)
sw = Pin(33, Pin.IN, Pin.PULL_UP)



# =====================================================
# BUZZER
# =====================================================

buzzer = PWM(Pin(2))
buzzer.freq(2000)
buzzer.duty(0)



# =====================================================
# MPU6050
# =====================================================

i2c = I2C(
    0,
    scl=Pin(22),
    sda=Pin(21)
)

MPU_ADDR = 0x68


# Wake up MPU6050
i2c.writeto_mem(
    MPU_ADDR,
    0x6B,
    b'\x00'
)



# =====================================================
# SYSTEM VARIABLES
# =====================================================

current_floor = 0
target_floor = 0


MAX_FLOOR = 8
MIN_FLOOR = 0


floor_changed = False
home_pressed = False


last_clk = clk.value()



# Motor status
motor_running = False
stop_motor = False



# Vibration state
vibration_state = False


STEPS_PER_FLOOR = 200

VIBRATION_LIMIT = 0.5



# =====================================================
# ENCODER INTERRUPT
# =====================================================

def encoder_callback(pin):

    global target_floor
    global last_clk
    global floor_changed


    clk_state = clk.value()


    if clk_state != last_clk and clk_state == 0:


        if dt.value() != clk_state:

            target_floor += 1

        else:

            target_floor -= 1



        if target_floor > MAX_FLOOR:

            target_floor = MAX_FLOOR


        if target_floor < MIN_FLOOR:

            target_floor = MIN_FLOOR



        floor_changed = True


    last_clk = clk_state




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
)



# =====================================================
# MPU FUNCTIONS
# =====================================================

def read_vibration():


    data = i2c.readfrom_mem(
        MPU_ADDR,
        0x3B,
        6
    )


    ax, ay, az = struct.unpack(
        ">hhh",
        data
    )


    ax = ax / 16384
    ay = ay / 16384


    vibration = max(
        abs(ax),
        abs(ay)
    )


    return vibration




def check_vibration():


    global vibration_state
    global stop_motor


    vibration = read_vibration()



    if vibration > VIBRATION_LIMIT:


        if vibration_state == False:

            print(
                "WARNING: High Vibration Detected"
            )


        vibration_state = True


        buzzer.duty(512)


        stop_motor = True



    else:


        if vibration_state == True:

            print(
                "Vibration Normal - Elevator Safe"
            )


        vibration_state = False


        buzzer.duty(0)


        stop_motor = False




# =====================================================
# STEPPER CONTROL
# =====================================================

def step_motor(steps, direction):

    global motor_running


    DIR.value(direction)

    motor_running = True


    for i in range(steps):


        if stop_motor:

            print(
                "Motor Stopped Due To Vibration"
            )

            break



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


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



    motor_running = False




def move_to_floor(floor):

    global current_floor


    if floor == current_floor:

        return



    print("----------------")
    print(
        "Moving Elevator"
    )

    print(
        "From Floor:",
        current_floor
    )


    print(
        "To Floor:",
        floor
    )



    difference = floor - current_floor


    steps = abs(difference) * STEPS_PER_FLOOR



    if difference > 0:

        step_motor(
            steps,
            1
        )

    else:

        step_motor(
            steps,
            0
        )



    if stop_motor == False:


        current_floor = floor


        print(
            "Reached Floor:",
            current_floor
        )



# =====================================================
# START
# =====================================================

print("======================")
print("Smart Elevator Ready")
print("Current Floor:", current_floor)
print("======================")



# =====================================================
# MAIN LOOP
# =====================================================

while True:


    # Always monitor MPU
    check_vibration()

    # Home button
    if home_pressed :
        print("Home floor pressed")
        target_floor = 0
        home_pressed = False

    # Encoder floor change
    if floor_changed :
        print("Selected floor : ", target_floor)
        if vibration_state == False:
            move_to_floor(target_floor)
        else:
            print("Can Not move - VIBRATION ALARM DETECTED")
        floor_changed = False

    time.sleep_ms(50)