# Messung der Winkelgeschwindigkeit

from MPU6050 import MPU6050
from machine import Pin
from time import sleep_ms, sleep, time
import math
import network # für WLAN Object 
from umqtt.robust import MQTTClient #für den MQTT Client 
import ujson #zum generieren eines JSON Strings 

mpu = MPU6050()

delay_time=0.5
gx_abs=0
gy_abs=0
gz_abs=0
prev_time=0

#WIFI connection
print("Connecting to WLAN ...")

sta_if = network.WLAN(network.STA_IF) #WLAN Object
sta_if.active(True) #activate Wlan
sta_if.connect('Wokwi-GUEST','')
while  not sta_if.isconnected():
    print(".", end = "")
    sleep(0.1)

print("WIFI connected")


#MQTT Setup
SERVER = "industrial.api.ubidots.com"
port = 1883
client_name = "raumlage"
topic = b"/v1.6/devices/raumlage"
token = "BBUS-4BrQmCUCJ42jwe0UOC9goTfYD3QDld"

#MQTT client Object
client = MQTTClient(client_name, SERVER, port, user=token, password=token)

while True:

    # Gyroscope Data
    gyro = mpu.read_gyro_data()   # read the gyro [deg/s]
    gX = gyro["x"]
    gY = gyro["y"]
    gZ = gyro["z"]


    #Berechnung der Winkeländerung basierend auf die aktuelle Winkelgesch.
    #und berechnung des aktuellen  absoluten Winkel, 1.x 2.y 3.z
    delta_gx=gX*delay_time
    gx_abs=gx_abs+delta_gx

    delta_gy=gY*delay_time
    gy_abs=gy_abs+delta_gy

    delta_gz=gZ*delay_time
    gz_abs=gz_abs+delta_gz

    print("x:" + str(gX) + " y:" + str(gY) + " z:" + str(gZ)+"            "+"x:" + str(gx_abs) + " y:" + str(gy_abs) + " z:" + str(gz_abs)      +"             "     +   angles_str)
 
    #ujson strings für die übertragung
    msg_gx = ujson.dumps({"Angle_x" :gx_abs})
    msg_gy = ujson.dumps({"Angle_y" :gy_abs})
    msg_gz = ujson.dumps({"Angle_z" :gz_abs})

    #alle 3s werden die nachrichten gepublisht, ohne delay um den Berechnungsprozess nicht zu stören.
    if time()-prev_time > 3:
        #Daten an den MQTT Server senden
        client.connect() #client verbinden
        client.publish(topic, msg_gx) #Nachricht beim richtigen Topic publishen 
        client.publish(topic, msg_gy) #Nachricht beim richtigen Topic publishen 
        client.publish(topic, msg_gz) #Nachricht beim richtigen Topic publishen 
        client.disconnect()#client disconnecten
        prev_time=time()

    # Time Interval Delay in sekunden
    sleep(delay_time)