from machine import Pin, I2C, PWM
import time
from hcsr04 import HCSR04  # Ultrasonic sensor driver for MicroPython
from servo import Servo     # Servo motor driver for MicroPython
from lcd_api import LcdApi
from i2c_lcd import I2cLcd

# Constants
MAX_DISTANCE = 200
PARKING_THRESHOLD = 30

# Pins
TRIG_PIN1 = 5
ECHO_PIN1 = 18
TRIG_PIN3 = 32
ECHO_PIN3 = 33

LED1_GREEN = 2
LED1_RED = 4
LED3_GREEN = 25
LED3_RED = 26

IR_SENSOR_PIN = 14
SERVO_PIN = 27

# Setup ultrasonic sensors
sonar1 = HCSR04(trigger_pin=TRIG_PIN1, echo_pin=ECHO_PIN1)
sonar3 = HCSR04(trigger_pin=TRIG_PIN3, echo_pin=ECHO_PIN3)

# Setup LEDs
led1_green = Pin(LED1_GREEN, Pin.OUT)
led1_red = Pin(LED1_RED, Pin.OUT)
led3_green = Pin(LED3_GREEN, Pin.OUT)
led3_red = Pin(LED3_RED, Pin.OUT)

# Setup IR sensor
ir_sensor = Pin(IR_SENSOR_PIN, Pin.IN)

# Setup servo
servo_pwm = PWM(Pin(SERVO_PIN), freq=50)
gate_servo = Servo(servo_pwm)

# Setup I2C LCD
I2C_ADDR = 0x27
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)  # ESP32 default I2C pins
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)

available_slots = 2

def set_leds(slot, occupied):
    if slot == 1:
        led1_red.value(occupied)
        led1_green.value(not occupied)
    elif slot == 3:
        led3_red.value(occupied)
        led3_green.value(not occupied)

def update_lcd():
    occupied_slots = 2 - available_slots
    lcd.clear()
    lcd.putstr("Smart Parking\n")
    lcd.putstr("Avail:{} Occ:{}".format(available_slots, occupied_slots))

def setup():
    # Initialize servo to closed position
    gate_servo.write_angle(0)
    time.sleep(1)
    # Initialize LEDs to OFF (green on)
    for slot in [1, 3]:
        set_leds(slot, False)
    update_lcd()

def loop():
    global available_slots

    # Measure distances
    try:
        dist1 = sonar1.distance_cm()
    except OSError:
        dist1 = None
    try:
        dist3 = sonar3.distance_cm()
    except OSError:
        dist3 = None

    occupied1 = dist1 is not None and dist1 < PARKING_THRESHOLD
    occupied3 = dist3 is not None and dist3 < PARKING_THRESHOLD

    set_leds(1, occupied1)
    set_leds(3, occupied3)

    available_slots = 0
    if not occupied1:
        available_slots += 1
    if not occupied3:
        available_slots += 1

    update_lcd()

    # Check IR sensor and open gate if needed
    if ir_sensor.value() == 0:  # IR sensor triggered (assuming LOW means detected)
        if available_slots > 0:
            gate_servo.write_angle(90)  # Open gate
            time.sleep(3)
            gate_servo.write_angle(0)   # Close gate

    time.sleep(0.5)

# Run the setup
setup()

# Main loop
while True:
    loop()