from machine import Pin
from time import sleep
import time
import network
from hcsr04 import HCSR04

try:
    import usocket as socket
except:
    import socket


print("\n############ Let's start ############\n")
###### Initialize ##########
button = Pin(21, Pin.IN, Pin.PULL_UP) # initialize Button with internal pull up resistor
sensor = HCSR04(trigger_pin=5, echo_pin=18) # initialize TOF Sensor
sensor_active = False # when sensor is active
sensor_last_state = False # saves sensors last state
detection = False # detectet Person
first_detect = 0 # time tick of first detection


##### Setup #################
ssid = "Wokwi-GUEST" #WLAN SSID
pw = "" # WLAN password
UDP_IP = "192.168.178.101" # UDP receiver ip-adress
UDP_PORT = 5005 # UDP receiver port
message = "esp32_send_message"

dst = 200 # set distance for detection




##### WLAN Setup ##################


wlan = network.WLAN(network.STA_IF)
wlan.active(True)
print(wlan.scan()) # print detectet WLAN
wlan.connect(ssid, pw)
print("Connect to wifi...", end="")
while not wlan.isconnected():
    print(".", end="")
    sleep(0.3)
print("\nYeah, connected!")
print(wlan.ifconfig())  # actual Connection Infos

def send_message():
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 
    sock.sendto(bytes(message, "utf-8"), (UDP_IP, UDP_PORT))
    print(f"Sent Message '{message}' to {UDP_IP} on Port {UDP_PORT}")

while True: # measure Distance
    distance = sensor.distance_cm() #read sensor
    mode = 0 # tell state in console (formated print statement down below)
    
    if distance < dst and detection == False: # reset first_detect at specified distance
        mode = 1
        detection = True
        first_detect = time.ticks_ms()

    if distance < dst and detection == True and time.ticks_diff(time.ticks_ms(), first_detect) > 3000:
        mode = 2
        sensor_active = True

    if distance >= dst and detection == True:
        mode = 3
        detection = False
        sensor_active = False
        first_detect = 0

    if button.value() == 0:
        mode = 4
        sensor_active = True
        sleep(0.05)
    if button.value() == 1 and detection == False:
        sensor_active = False
    
    if sensor_active and not sensor_last_state:
        sensor_last_state = True
        send_message()

    if not sensor_active and sensor_last_state:
        sensor_last_state = False

    print(f"Mode:{mode} | Distance: {distance}cm | now:{time.ticks_ms()} / first_detect:{first_detect} | Detection={detection} | sensor:{sensor_active}")

    sleep(0.2)

print("\n############ Shut down ############\n")