from machine import Pin, I2C, PWM
from utime import sleep, sleep_ms, sleep_us, ticks_ms, ticks_diff, ticks_us, time
from i2c_lcd import I2cLcd
import network
from umqtt_simple import MQTTClient
import json

# Constants
WIFI_SSID = "Wokwi-GUEST"
WIFI_PASSWORD = ""
ADAFRUIT_IO_USERNAME = "tharunnv"
ADAFRUIT_AIO_KEY = "aio_uqnk18hiIB1r16iUxRXq24U49BcL"
AIO_SERVER = "io.adafruit.com"
AIO_PORT = 1883

# Pin Configurations
TRIGGER_PIN = 12
ECHO_PIN = 13
SERVO_PIN = 17
LCD_SDA_PIN = 20
LCD_SCL_PIN = 21
I2C_ADDR = 0x27

# Keypad Configuration
ROW_PINS = [2, 3, 4, 5]
COL_PINS = [6, 7, 8, 9]
KEYPAD_MATRIX = [
    ['1', '2', '3', 'A'],
    ['4', '5', '6', 'B'],
    ['7', '8', '9', 'C'],
    ['*', '0', '#', 'D']
]

# PIN for manual override
MANUAL_OVERRIDE_PIN = 'D'
EMERGENCY_STOP_PIN = '*'

class ParkingSystem:
    def __init__(self):
        self.initialize_hardware()
        self.connect_wifi()
        self.connect_mqtt()
        self.pin = "1234"  # Default PIN for manual override
        self.manual_mode = False
        self.emergency_stop = False

    def initialize_hardware(self):
        # Initialize LCD
        i2c = I2C(0, sda=Pin(LCD_SDA_PIN), scl=Pin(LCD_SCL_PIN), freq=400000)
        self.lcd = I2cLcd(i2c, I2C_ADDR, 4, 20)

        # Initialize Ultrasonic Sensor
        self.ultrasonic_trigger = Pin(TRIGGER_PIN, Pin.OUT)
        self.ultrasonic_echo = Pin(ECHO_PIN, Pin.IN)
        self.ultrasonic_trigger.value(0)  # Ensure trigger starts LOW

        # Initialize Servo with correct frequency
        self.servo = PWM(Pin(SERVO_PIN))
        self.servo.freq(50)

        # Initialize Keypad
        self.rows = [Pin(pin, Pin.OUT) for pin in ROW_PINS]
        self.cols = [Pin(pin, Pin.IN, Pin.PULL_UP) for pin in COL_PINS]

    def connect_wifi(self):
        self.wlan = network.WLAN(network.STA_IF)
        self.wlan.active(True)
        if not self.wlan.isconnected():
            self.lcd.putstr("Connecting WiFi...")
            self.wlan.connect(WIFI_SSID, WIFI_PASSWORD)
            while not self.wlan.isconnected():
                sleep(1)
            self.lcd.clear()
            self.lcd.putstr("WiFi Connected!")

    def connect_mqtt(self):
        self.mqtt = MQTTClient(
            client_id=ADAFRUIT_IO_USERNAME,
            server=AIO_SERVER,
            port=AIO_PORT,
            user=ADAFRUIT_IO_USERNAME,
            password=ADAFRUIT_AIO_KEY
        )
        self.mqtt.connect()

    def read_distance(self):
        self.ultrasonic_trigger.low()
        sleep_us(2)
        self.ultrasonic_trigger.high()
        sleep_us(10)
        self.ultrasonic_trigger.low()
        timeout_start = ticks_us()
        while self.ultrasonic_echo.value() == 0:
            if ticks_diff(ticks_us(), timeout_start) > 30000:
                return -1
        start = ticks_us()
        while self.ultrasonic_echo.value() == 1:
            if ticks_diff(ticks_us(), timeout_start) > 30000:
                return -1
        end = ticks_us()
        return (ticks_diff(end, start) * 0.0343) / 2

    def control_servo(self, angle):
        try:
            # Map angle to duty cycle based on provided values
            duty_cycle_map = {
                0: 1638,
                45: 3276,
                90: 4915,
                135: 6554,
                180: 8192
            }
            duty = duty_cycle_map.get(angle, int((angle / 180) * (8192 - 1638) + 1638))
            self.servo.duty_u16(duty)
            print(f"Servo moved to {angle}° with duty cycle {duty}")
            sleep(1)  # Adjust delay based on servo motor speed
        except Exception as e:
            print(f"Error controlling servo: {e}")

    def read_keypad(self):
        for i, row in enumerate(self.rows):
            row.value(0)
            sleep_ms(1)  # Debounce
            for j, col in enumerate(self.cols):
                if col.value() == 0:
                    row.value(1)
                    sleep_ms(20)  # Debounce
                    return KEYPAD_MATRIX[i][j]
            row.value(1)
        return None

    def update_display(self, status, distance=None):
        self.lcd.clear()
        self.lcd.putstr(f"Status: {status}")
        if distance is not None:
            self.lcd.move_to(0, 1)
            self.lcd.putstr(f"Distance: {distance:.1f}cm")

    def publish_status(self, status):
        try:
            feed = f"{ADAFRUIT_IO_USERNAME}/feeds/parking-status"
            self.mqtt.publish(feed, json.dumps({"status": status}))
        except Exception as e:
            print(f"MQTT Error: {e}")
            self.connect_mqtt()

    def check_pin(self):
        entered_pin = ""
        self.lcd.clear()
        self.lcd.putstr("Enter PIN:")
        while len(entered_pin) < 4:
            key = self.read_keypad()
            if key:
                entered_pin += key
                self.lcd.move_to(len(entered_pin) + 6, 1)
                self.lcd.putstr('*')
                sleep_ms(300)
        return entered_pin

    def main_loop(self):
        print("Starting main loop...")
        last_distance = None
        while True:
            try:
                # Measure distance
                distance = self.read_distance()
                if distance != -1:
                    print(f"Distance: {distance:.1f}cm")  # Debug print
                    if not self.manual_mode and not self.emergency_stop:
                        if distance < 10:
                            # Car detected
                            print("Car detected - Closing barrier")  # Debug print
                            self.control_servo(90)  # Close barrier
                            self.update_display("OCCUPIED", distance)
                            if last_distance is None or last_distance >= 10:
                                self.publish_status("occupied")
                        else:
                            print("No car - Opening barrier")  # Debug print
                            self.control_servo(0)  # Open barrier
                            self.update_display("AVAILABLE", distance)
                            if last_distance is None or last_distance < 10:
                                self.publish_status("available")
                        last_distance = distance

                # Check keypad
                key = self.read_keypad()
                if key:
                    print(f"Key pressed: {key}")  # Debug print
                    if key == MANUAL_OVERRIDE_PIN:
                        self.lcd.clear()
                        self.lcd.putstr("Manual Mode")
                        entered_pin = self.check_pin()
                        if entered_pin == self.pin:
                            self.manual_mode = True
                            self.lcd.clear()
                            self.lcd.putstr("Manual Mode ON")
                            sleep(2)
                        else:
                            self.lcd.clear()
                            self.lcd.putstr("Wrong PIN!")
                            sleep(2)
                    elif key == EMERGENCY_STOP_PIN:
                        self.emergency_stop = True
                        self.control_servo(90)  # Close barrier
                        self.lcd.clear()
                        self.lcd.putstr("Emergency Stop!")
                        sleep(2)
                    elif self.manual_mode:
                        if key == 'A':
                            self.control_servo(0)  # Open barrier
                            self.update_display("MANUAL OPEN")
                        elif key == 'B':
                            self.control_servo(90)  # Close barrier
                            self.update_display("MANUAL CLOSE")
                        elif key == 'C':
                            self.manual_mode = False
                            self.lcd.clear()
                            self.lcd.putstr("Manual Mode OFF")
                            sleep(2)
                    else:
                        self.lcd.move_to(0, 2)
                        self.lcd.putstr(f"Key: {key}")
                        sleep_ms(100)  # Small delay to prevent overwhelming the system
            except Exception as e:
                print(f"Error in main loop: {e}")
                self.lcd.clear()
                self.lcd.putstr("Error occurred\nRetrying...")
                sleep(2)

if __name__ == "__main__":
    print("Initializing Parking System...")
    parking_system = ParkingSystem()
    parking_system.main_loop()