from machine import Pin, I2C
import ssd1306
import mpu6050
import time
import math
import network
from umqtt.simple import MQTTClient

# MQTT Server Parameters
MQTT_CLIENT_ID = "anemo_test"
MQTT_BROKER    = "broker.hivemq.com"
MQTT_USER      = ""
MQTT_PASSWORD  = ""
MQTT_TOPIC     = "wind"

# WIFI Connection
print("Connecting to WiFi", end="")
sta_if = network.WLAN(network.STA_IF)
sta_if.active(True)
sta_if.connect('Wokwi-GUEST', '')
while not sta_if.isconnected():
  print(".", end="")
  time.sleep(0.1)
print(" Connected!")

# MQTT Server connection
print("Connecting to MQTT server... ", end="")
client = MQTTClient(MQTT_CLIENT_ID, MQTT_BROKER, user=MQTT_USER, password=MQTT_PASSWORD)
client.connect()
print("Connected!")

# ESP32 Pin assignment 
i2c = I2C(0, scl=Pin(40), sda=Pin(41))

oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

mpu = mpu6050.MPU6050(i2c)

# wake up the MPU6050 from sleep
mpu.wake()
gyro = ""
accel = ""
temp = ""
pitch = ""
roll = ""
wdir = ""
wspd = ""
wdstr = "O"
# continuously print the data
while True:
    new_accel = mpu.read_accel_data()
    if(new_accel != accel):
      accel = new_accel
      pitch = math.atan(accel[0]/accel[2])
      roll = math.atan(accel[1]/accel[2])
      if roll > 0:
        wdir = math.atan2(roll,pitch)*180/math.pi
      else:
        wdir = math.atan2(-roll,-pitch)*180/math.pi+180

      wspd = math.sqrt(pow(pitch,2) + pow(roll,2))

      if(wdir < 11.25):
        wdstr = "N"
      elif(wdir < 33.75):
        wdstr = "NNA"
      elif(wdir < 56.25):
        wdstr = "NA"
      elif(wdir < 78.75):
        wdstr = "ANA"
      elif(wdir < 101.25):
        wdstr = "A"
      elif(wdir < 123.75):
        wdstr = "ASA"
      elif(wdir < 146.25):
        wdstr = "SA"
      elif(wdir < 168.75):
        wdstr = "SSA"
      elif(wdir < 191.25):
        wdstr = "S"
      elif(wdir < 213.75):
        wdstr = "SSV"
      elif(wdir < 236.25):
        wdstr = "SV"
      elif(wdir < 258.75):
        wdstr = "VSV"
      elif(wdir < 281.25):
        wdstr = "V"
      elif(wdir < 303.75):
        wdstr = "VNV"
      elif(wdir < 326.25):
        wdstr = "NV"
      elif(wdir < 348.75):
        wdstr = "NNV"
      else:
        wdstr = "N"

      #print("Wdir: " + str(int(wdir)) + " Wspd: " + str(wspd))

      oled.fill(0)
      oled.text("Wind speed: " + str(wspd), 0, 15)
      oled.text("Wind dir:   " + wdstr    , 0, 35)       
      oled.show()

      message = "Wind: " + wdstr + " " + str(round(wspd,2))
      print(message)
      
      client.publish(MQTT_TOPIC, message)

    time.sleep(1)