from machine import Pin, I2C, PWM
from ssd1306 import SSD1306_I2C
import utime

pix_res_x = 128
pix_res_y = 64

def init_i2c(scl_pin, sda_pin):
    # Initialize I2C device
    i2c_dev = I2C(1, scl=Pin(scl_pin), sda=Pin(sda_pin), freq=200000)
    i2c_addr = [hex(ii) for ii in i2c_dev.scan()]
    
    if not i2c_addr:
        print('No I2C Display Found')
        sys.exit()
    else:
        print("I2C Address      : {}".format(i2c_addr[0]))
        print("I2C Configuration: {}".format(i2c_dev))
    
    return i2c_dev

def measure_distance(trigger_pin, echo_pin):
    # Measure distance using HC-SR04 ultrasonic sensor
    trigger = Pin(trigger_pin, Pin.OUT)
    echo = Pin(echo_pin, Pin.IN)

    # Send a 10us pulse to trigger
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(10)
    trigger.low()

    # Measure the time for the echo signal
    while echo.value() == 0:
        start_time = utime.ticks_us()
    while echo.value() == 1:
        end_time = utime.ticks_us()

    # Calculate distance (time * speed of sound / 2)
    elapsed_time = utime.ticks_diff(end_time, start_time)
    distance = (elapsed_time * 0.0343) / 2

    return distance

def operate_parking_system(oled, trigger_pin, echo_pin, switch1_pin, switch2_pin, servo_pin, red_led_pin, yellow_led_pin, green_led_pin, buzzer_pin):
    switch1 = Pin(switch1_pin, Pin.IN, Pin.PULL_DOWN)
    switch2 = Pin(switch2_pin, Pin.IN, Pin.PULL_DOWN)
    servo = PWM(Pin(servo_pin))
    servo.freq(50)  # Set frequency for the servo motor
    
    # LEDs and Buzzer
    red_led = Pin(red_led_pin, Pin.OUT)
    yellow_led = Pin(yellow_led_pin, Pin.OUT)
    green_led = Pin(green_led_pin, Pin.OUT)
    buzzer = Pin(buzzer_pin, Pin.OUT)
    
    yellow_led.on()  # Turn on yellow LED when system starts
    
    while True:
        distance = measure_distance(trigger_pin, echo_pin)

        if distance > 20 and switch1.value() == 0 and switch2.value() == 0:
            oled.fill(0)
            oled.text("IR Sensor Operating", 5, 5)
            oled.text("Waiting for Car...", 5, 15)
            oled.show()
            
            # Blink red LED if distance is below 20cm and switch 1 hasn't been triggered in 10 seconds
            if switch1.value() == 0 and distance <= 20:
                red_led.on()
                utime.sleep(0.5)
                red_led.off()
                utime.sleep(0.5)

                # Check if switch1 hasn't been pressed for 10 seconds
                start_time = utime.ticks_ms()
                while utime.ticks_diff(utime.ticks_ms(), start_time) < 10000:
                    if switch1.value() == 1:
                        break
                    red_led.on()
                    utime.sleep(0.5)
                    red_led.off()
                    utime.sleep(0.5)

                if utime.ticks_diff(utime.ticks_ms(), start_time) >= 10000:
                    # Operate buzzer after 10 seconds if switch1 was not pressed
                    buzzer.on()
                    utime.sleep(1)
                    buzzer.off()
                    utime.sleep(1)

        elif distance <= 20 and switch1.value() == 0 and switch2.value() == 0:
            oled.fill(0)
            oled.text("Car Detected", 5, 5)
            oled.show()
            utime.sleep(2)
            oled.fill(0)
            oled.text("Start Face", 5, 5)
            oled.text("Recognition", 5, 15)
            oled.show()
        
        if switch1.value() == 1 and distance <= 20:
            oled.fill(0)
            oled.text("Car Plate", 5, 5)
            oled.text("Detected", 5, 15)
            oled.text("Start Face", 5, 25)
            oled.text("Recognition", 5, 35)
            oled.show()
            
        if switch2.value() == 1 and switch1.value() == 1 and distance <= 20:
            oled.fill(0)
            oled.text("Driver Face", 5, 5)
            oled.text("Detected", 5, 15)
            oled.show()
            print("Switches pressed, operating servo...")

            # Turn on green LED when the servo operates
            yellow_led.off()
            green_led.on()

            # Open servo motor
            servo.duty_u16(8000)  # Adjust duty cycle to open servo
            utime.sleep(7)
            
            oled.fill(0)
            oled.text("Welcome!", 5, 5)
            oled.show()
            utime.sleep(3)
            
            # Reset servo motor
            servo.duty_u16(1000)  # Adjust duty cycle to reset servo
            utime.sleep(7)

            # Turn off green LED after servo operation
            green_led.off()

def main():
    i2c_dev = init_i2c(scl_pin=27, sda_pin=26)
    oled = SSD1306_I2C(pix_res_x, pix_res_y, i2c_dev)

    # HC-SR04 pins
    trigger_pin = 8
    echo_pin = 7

    # Switch pins
    switch1_pin = 4
    switch2_pin = 5

    # Servo pin
    servo_pin = 6

    # LED and Buzzer pins
    red_led_pin = 0
    yellow_led_pin = 1
    green_led_pin = 2
    buzzer_pin = 3

    oled.fill(0)
    oled.text("Parking System", 5, 5)
    oled.text("Initializing...", 5, 15)
    oled.show()
    utime.sleep(2)

    operate_parking_system(oled, trigger_pin, echo_pin, switch1_pin, switch2_pin, servo_pin, red_led_pin, yellow_led_pin, green_led_pin, buzzer_pin)

if __name__ == '__main__':
    main()