import machine, math
import MPU6050 as mpu6050
from machine import Pin

led1 = machine.Pin(32, machine.Pin.OUT)
led2 = machine.Pin(2, machine.Pin.OUT)
led3 = machine.Pin(33, machine.Pin.OUT)
led4 = machine.Pin(4, machine.Pin.OUT)
led5 = machine.Pin(25, machine.Pin.OUT)
led6 = machine.Pin(26, machine.Pin.OUT)


sdaPIN=machine.Pin(21) ; sclPIN=machine.Pin(22)   # ESP32

i2c=machine.I2C(sda=sdaPIN, scl=sclPIN, freq=10000)   
devices = i2c.scan()
if len(devices) == 0:
 print("No i2c device !")
else:
  print('i2c devices found:',len(devices))
  for device in devices:
    print("At address: ",hex(device))
print("\nEnde")
#i2c = I2C(scl=Pin(22), sda=Pin(21))     #initializing the I2C method for ESP32
#i2c = I2C(scl=Pin(5), sda=Pin(4))       #initializing the I2C method for ESP8266
mpu = mpu6050.accel(i2c)
while True:
  vals = mpu.get_values()

  if vals["AcX"]==0:
    if vals["AcY"]==0:
      if vals["AcZ"]==0:
        led1.value(1)
        led2.value(1)
        led3.value(1)
        led4.value(1)
        led5.value(1)
        led6.value(1)
      else:
        led1.value(0)
        led2.value(0)
        led3.value(0)
        led4.value(0)
        led5.value(0)
        led6.value(0)
    else:
      led1.value(0)
      led2.value(0)
      led3.value(0)
      led4.value(0)
      led5.value(0)
      led6.value(0)
  else:
    led1.value(0)
    led2.value(0)
    led3.value(0)
    led4.value(0)
    led5.value(0)
    led6.value(0)
    
  if vals["AcX"]>0:
    led1.value(0)
    led2.value(1)
    led3.value(0)
    led4.value(1)
  
  if vals["AcX"]<0:
    led1.value(1)
    led2.value(0)
    led3.value(1)
    led4.value(0)

  if vals["AcZ"]>0:
    led1.value(0)
    led2.value(0)
    led3.value(1)
    led4.value(1)

  if vals["AcZ"]<0:
    led1.value(1)
    led2.value(1)
    led3.value(0)
    led4.value(0)
  
  if vals["AcY"]>0:
    led1.value(0)
    led2.value(0)
    led3.value(0)
    led4.value(0)
    led5.value(0)
    led6.value(1)

  if vals["AcY"]<0:
    led1.value(0)
    led2.value(0)
    led3.value(0)
    led4.value(0)
    led5.value(1)
    led6.value(0)

  #print(vals)
  if vals["AcX"]!=0:
    winkelxz = math.atan(vals["AcZ"]/vals["AcX"])/math.pi*180
  else:
    if vals["AcZ"]>=0:
      winkelxz = 90
    else:
      winkelxz = -90
  print(vals["AcX"],vals["AcY"],vals["AcZ"],winkelxz)
  

machine.sleep(500)