print("Hello, ESP32!")


#import libraries/modules
from machine import Pin, PWM
import ultrasonic_library
import servo_library
from utime import sleep, ticks_ms, ticks_diff

#pin declaration
TRIG1 = Pin(13)
TRIG2 = Pin(5)
ECHO1 = Pin(27)
ECHO2 = Pin(18)
red1 = PWM(Pin(14), freq=5000)
green1 = PWM(Pin(12), freq=5000)
blue1 = PWM(Pin(26), freq=5000)
red2 = PWM(Pin(17), freq=5000)
green2 = PWM(Pin(16), freq=5000)
blue2 = PWM(Pin(4), freq=5000)
servo_pin = PWM(Pin(33), freq=50)


#Object Declaration
#Is only for component with library
#ObjectName = LibraryName.Classname(.....)
ultrasonic_sensor1 = ultrasonic_library.HCSR04(trigger_pin = TRIG1, echo_pin = ECHO1)
ultrasonic_sensor2 = ultrasonic_library.HCSR04(trigger_pin = TRIG2, echo_pin = ECHO2)


def set_servo_angle(angle):
    duty = int(((angle / 180) * 100) + 26)
    servo_pin.duty(duty)

def set_color1(r, g, b):
    # Duty cycle ranges from 0 to 1023 in MicroPython
    red1.duty(r)
    green1.duty(g)
    blue1.duty(b)

def set_color2(r, g, b):
    # Duty cycle ranges from 0 to 1023 in MicroPython
    red2.duty(r)
    green2.duty(g)
    blue2.duty(b)

# Pemasa untuk menjejak tempoh "Jauh" (dalam milisaat)
jauh_start_time = None



# Set posisi asal 
set_servo_angle(0)



#main program
while True:
    #Start measure distance 
    object_distance_mm1 = ultrasonic_sensor1.distance_mm()
    object_distance_mm2 = ultrasonic_sensor2.distance_mm()


    print("Distance from chair : ",object_distance_mm1, "mm")
    print("................................................")
    

    


    if object_distance_mm2 > 3000:
        set_color2(1023, 0, 0) #red
        

    elif object_distance_mm2 <= 100:
        set_color2(0, 1023, 0)#green 
        

    if object_distance_mm1 > 3000:
        set_color1(1023, 0, 0) #red
        

    elif object_distance_mm1 <= 100:
        set_color1(0, 1023, 0)#green
        

    # Syarat A: Jika SALAH SATU sensor mengesan objek JAUH (> 3000 mm)
    if object_distance_mm1 > 3000 or object_distance_mm2 > 3000:
        if jauh_start_time is None:
            jauh_start_time = ticks_ms() # Rekod masa mula dikesan jauh (dalam milisaat)
        else:
            # Kira berapa lama masa telah berlalu (milisaat)
            elapsed_time = ticks_diff(ticks_ms(), jauh_start_time)
            
            if elapsed_time >= 5000: # 5000 milisaat = 5 saat
                set_servo_angle(0) # Servo kembali/kekal ke 0 darjah (Lock)
                print("STATUS: LOCK AKTIF (Sensor jauh melebihi 5 saat!)")

    # Syarat B: Jika salah satu atau kedua-dua sensor mengesan objek DEKAT (<= 100 mm)
    elif object_distance_mm1 <= 100 or object_distance_mm2 <= 100:
        jauh_start_time = None # Reset pemasa jauh
        set_servo_angle(180)   # Servo bergerak patah balik ke 180 darjah
        print("STATUS: UNLOCK (Objek dekat, Servo ke 180°)")
        
    else:
        # Jika jarak berada di tengah-tengah (antara 100mm dan 3000mm), reset pemasa
        jauh_start_time = None

    sleep(0.2)