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"""
    # 1. Retract lock pin (Servo 1 opens to 90°)
    set_servo(lock_servo, 90)
    
    # 2. Delay for looking/unlocking sequence 
    time.sleep_ms(500) 
    
    # 3. Swing door panel open (Servo 2 opens to 180°)
    set_servo(door_servo, 180)

def close_sequence():
    """Locking sequence: Close door panel -> Delay -> Engage lock pin"""
    # 1. Close door panel (Servo 2 returns to 0°)
    set_servo(door_servo, 0)
    
    # 2. Delay for looking/locking sequence (allows door to fully sit in frame)
    time.sleep_ms(800) 
    
    # 3. Engage lock pin (Servo 1 returns to 0°)
    set_servo(lock_servo, 0)

# ---------------- 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  # Track state to avoid looping the sequence repeatedly

# Initialize everything to closed state at startup
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 changes
    if abs(dist - last_distance) >= 2:
        last_distance = dist
    else:
        dist = last_distance

    # Logic evaluation
    if gas:
        led.on()
        open_sequence()
        door_currently_open = True
        door = "OPEN"
    elif dist <= 30:
        led.off()
        if not door_currently_open:
            open_sequence()
            door_currently_open = True
        door = "OPEN"
    else:
        led.off()
        if door_currently_open:
            close_sequence()
            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)