from machine import Pin, time_pulse_us, PWM, I2C
import time
time.sleep(0.1) # Wait for USB to become ready

#       Pins Configuration:
# 1. Front Ultrasonic Sensor(Trig) = 2
# 2. Front Ultrasonic Sensor(Echo) = 3
# 3. Back Ultrasonic Sensor(Trig) = 4
# 4. Back Ultrasonic Sensor(Echo) = 5
# 5. Left Ultrasonic Sensor(Trig) = 6
# 6. Left Ultrasonic Sensor(Echo) = 7
# 7. Right Ultrasonic Sensor(Trig) = 8
# 8. Right Ultrasonic Sensor(Echo) = 9
# 9. Steering Servo = 10
# 10. L298N Int1(Movement Motor) = 11
# 11. L298N Int2(Left Swiping Motor) = 12
# 12. L298N Int3(Right Swiping Motor) = 13
# 13. Touch Sensor = 14
# 14. Buzzer = 15
# 15. 1.3Inch I2C OLED Display(SDA) = 16
# 16. 1.3Inch I2C OLED Display(SCL/SCK) = 17
# 17. DS3231 RTC Module(SDA) = 16
# 18. DS3231 RTC Module(SCL/SCK) = 17

led = PWM(Pin(25))
led.freq(1000)

# Ultrasonic sensors
frontTrig = Pin(2, Pin.OUT)
frontEcho = Pin(3, Pin.IN)
backTrig = Pin(4, Pin.OUT)
backEcho = Pin(5, Pin.IN)
leftTrig = Pin(6, Pin.OUT)
leftEcho = Pin(7, Pin.IN)
rightTrig = Pin(8, Pin.OUT)
rightEcho = Pin(9, Pin.IN)

# Steering Servo
steering = PWM(Pin(10))
steering.freq(50)

# L298N Motors
motorMove = Pin(11, Pin.OUT)
motorLeft = Pin(12, Pin.OUT)
motorRight = Pin(13, Pin.OUT)

# Touch sensor
touch = Pin(14, Pin.IN, Pin.PULL_DOWN)

# Buzzer
buzzer = PWM(Pin(15))
buzzer.freq(2000)  # initial frequency
buzzer.duty_u16(0)  # start off

Cleaning = False

# I2C pins (OLED + RTC)
# i2c = I2C(1, scl=Pin(17), sda=Pin(16))

def Cleaning_Function():

    motorMove = 

def fade_led(led_pwm, duration=2, steps=100):
    
    delay = duration / (2 * steps)  # half fade in, half fade out
    
    # Fade in
    for i in range(steps + 1):
        duty = int(i * 65535 / steps)  # PWM duty 0-65535
        led_pwm.duty_u16(duty)
        time.sleep(delay)
    
    # Fade out
    for i in range(steps, -1, -1):
        duty = int(i * 65535 / steps)
        led_pwm.duty_u16(duty)
        time.sleep(delay)

def getDistance(trig, echo):
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    try:
        duration = time_pulse_us(echo, 1, 30000)  # 30ms timeout
        distance = int(duration / 58)
    except OSError:
        distance = -1  # no reading
    return distance

while True:
    # Check if touch sensor is pressed AND we are idle
    if touch.value() == 1 and not Cleaning:
        Cleaning = True
        print("Cleaning started!")
        
        # Call cleaning routine
        # Example: move motors, use sensors, steer, etc.
        # This routine should run to completion and not check touch again
        fade_led(led, duration=2)
        
        # After cleaning finishes
        Cleaning = False
        print("Cleaning finished. Ready for next start.")
        
    time.sleep(0.1)  # small debounce

    d1 = getDistance(frontTrig, frontEcho)
    print(d1)
    time.sleep(0.1)