# network libraries
import network
import config
import hcsr04

# algemene bibliotheken
import time

# bibliotheken voor mqtt
from umqttsimple import MQTTClient

# variabelen voor mqtt
mqtt_server = 'test.mosquitto.org'
client_id = 'osvives56473875R4839Z'
topic_pub = b'osviveslabo3'

# variabelen voor wifi
WIFI_NAME=config.wifi_ssid
WIFI_PASSWORD=config.wifi_password

# functie om te connecteren met wifi
def connect_WiFi():
    print("Connecting to WiFi ", end="")
    sta_if = network.WLAN(network.STA_IF)
    sta_if.active(True)
    sta_if.connect(WIFI_NAME, WIFI_PASSWORD)
    while not sta_if.isconnected():
      print(".", end="")
      time.sleep(0.1)
    print("\nConnected!")

# functie om te connecteren met mqtt broker
def mqtt_connect():
    client = MQTTClient(client_id, mqtt_server, port=1883, user="", password="", keepalive=7200)
    client.connect()
    print('Connected to %s MQTT Broker'%(mqtt_server))
    return client

# functie om te herstarten indien geen wifi connectie
def reconnect():
    print('Failed to connect to the MQTT Broker. Reconnecting...')
    time.sleep(5)
    machine.reset()

try:
    connect_WiFi()
    client = mqtt_connect()
except OSError as e:
    print(e)
    reconnect()

# code om nummers te tellen en te publishen naar een topic
number = 0



# Pin assignments for the HC-SR04
TRIGGER_PIN = 6
ECHO_PIN = 7

# Instantiate the sensor object once before the loop
# (It's more efficient to create the object once, not every 5 seconds)
try:
    sensor = hcsr04.HCSR04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN)
except NameError:
    # Use hcsr04.HCSR04 if the import was 'import hcsr04'
    sensor = hcsr04.HCSR04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN) 
    
print("Starting distance measurement loop...")

while True:
    # 1. Measure the distance
    distance = sensor.distance_cm()
    
    # 2. Format the message for publishing
    # Format to one decimal place and convert the string to bytes
    msg = "{:.1f}".format(distance).encode('utf-8')
    
    # 3. Publish the message
    # topic_pub is defined as b'osviveslabo3'
    client.publish(topic_pub, msg)
    
    print('Distance:', distance, 'cm. Published:', msg)
    
    # 4. Wait for the next reading
    time.sleep(5)