import machine
from machine import Pin, UART, ADC, PWM
import dht
import time

# ---------------- Pins ----------------
DHT_PIN = 14
MQ2_PIN = 34

TRIG_PIN = 5
ECHO_PIN = 18

LOCK_SERVO_PIN = 27  
DOOR_SERVO_PIN = 25  
LED_PIN = 2

UART_TX = 17
UART_RX = 16

# ---------------- Devices ----------------
led = Pin(LED_PIN, Pin.OUT)

dht_sensor = dht.DHT22(Pin(DHT_PIN))

mq2 = ADC(Pin(MQ2_PIN))
mq2.atten(ADC.ATTN_11DB)

trigger = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)

# Initialize both Servos
lock_servo = PWM(Pin(LOCK_SERVO_PIN))
lock_servo.freq(50)

door_servo = PWM(Pin(DOOR_SERVO_PIN))
door_servo.freq(50)

uart = UART(1, baudrate=9600, tx=Pin(UART_TX), rx=Pin(UART_RX))

# ---------------- Servo Helper ----------------
def set_servo(servo_obj, angle):
    pulse = 500 + (angle / 180) * 2000
    duty = int((pulse / 20000) * 65535)
    servo_obj.duty_u16(duty)

# ---------------- Action Sequences ----------------
def open_sequence():
    """Unlocking sequence: Retract pin -> Delay -> Swing door open"""
    print("--- Starting Unlocking Sequence ---")
    # 1. Retract lock pin (Servo 1 opens to 90 degrees)
    set_servo(lock_servo, 90)
    
    # 2. Realistic mechanical delay to let the lock cylinder fully pull back
    time.sleep_ms(600) 
    
    # 3. Swing door panel open (Servo 2 opens to 180 degrees)
    set_servo(door_servo, 90)
    print("--- Door Fully Open ---")

def close_sequence():
    """Locking sequence: Close door panel -> Delay -> Engage lock pin"""
    print("--- Starting Locking Sequence ---")
    # 1. Close door panel first (Servo 2 returns to 0 degrees)
    set_servo(door_servo, 0)
    
    # 2. Realistic mechanical delay to let the door panel physically reach the frame
    time.sleep_ms(1000) 
    
    # 3. Engage lock pin safely into the closed door (Servo 1 returns to 0 degrees)
    set_servo(lock_servo, 0)
    print("--- Door Safely Locked ---")

# ---------------- Ultrasonic ----------------
def distance():
    trigger.off()
    time.sleep_us(2)
    trigger.on()
    time.sleep_us(10)
    trigger.off()

    try:
        pulse = machine.time_pulse_us(echo, 1, 30000)
        if pulse < 0:
            return 999
        return round(pulse / 58)
    except OSError:
        return 999

# ---------------- Main ----------------
GAS_THRESHOLD = 2200

previous_message = ""
last_distance = 999
door_currently_open = False  # Critical guard state variable

# Initialize physical system to a safe, closed state at power-on
close_sequence()

while True:
    try:
        dht_sensor.measure()
        temperature = dht_sensor.temperature()
        humidity = dht_sensor.humidity()
    except:
        temperature = 0
        humidity = 0

    gas_value = mq2.read()
    gas = gas_value > GAS_THRESHOLD

    dist = distance()

    # Ignore small distance fluctuations
    if abs(dist - last_distance) >= 2:
        last_distance = dist
    else:
        dist = last_distance

    # Determine structural system state requirement
    should_open = gas or (dist <= 30)

    # LED Indicator behavior
    if gas:
        led.on()
    else:
        led.off()

    # Execution State Machine
    if should_open:
        if not door_currently_open:
            open_sequence()  # Triggers once, lets the nested delays execute flawlessly
            door_currently_open = True
        door = "OPEN"
    else:
        if door_currently_open:
            close_sequence() # Triggers once, waiting for door panel to settle before dropping lock
            door_currently_open = False
        door = "CLOSED"

    # Pack and send data over UART
    message = "{:.1f},{:.1f},{},{},{},{}".format(
        temperature,
        humidity,
        dist,
        gas_value,
        int(gas),
        door
    )

    if message != previous_message:
        uart.write(message + "\n")
        print(message)
        previous_message = message

    time.sleep_ms(250)