from nanpy import ArduinoApi
from nanpy import SerialManager
from time import sleep
import serial




from nanpy.wire import Wire


def MPU_6050_read_data(SerialName):
    MPU_addr = 0x68  # I2C address of the MPU-6050
    b = 0

    Sera = str(SerialName)
    connection = SerialManager(device=Sera)    
    ard = Wire(connection=connection)

    ard.begin()
    ard.beginTransmission(MPU_addr)
    ard.write(0x6B)  
    ard.write(0x0)# set to zero (wakes up the MPU-6050)
    ard.endTransmission(True)
    print('Lecture des donnees du MPU-6050 en cours')

    while True:
    
        ard.beginTransmission(MPU_addr)
        ard.write(0x3B)  # starting with register 0x3B (ACCEL_XOUT_H)
        ard.endTransmission(False)
        ard.requestFrom(MPU_addr, 14, True)  # request a total of 14 registers
        AcX = ard.read() << 8 | ard.read()  # 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
        AcY = ard.read() << 8 | ard.read()  # 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
        AcZ = ard.read() << 8 | ard.read()  # 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
        Tmp = ard.read() << 8 | ard.read()  # 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
        GyX = ard.read() << 8 | ard.read()  # 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
        GyY = ard.read() << 8 | ard.read()  # 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
        GyZ = ard.read() << 8 | ard.read()  # 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
        print("AcX = ", AcX)
        print("AcX = ", bin(AcX))
        #print("AcXH = ", AcXH)
        #print("AcXH = ", bin(AcXH))
        #print("AcXL = ", AcXL)
        #print("AcXL = ", bin(AcXL))
        print("AcY = ", AcY)
        print("AcZ = ", AcZ)
        print("Tmp = ", Tmp / 340.00 + 36.53)  # equation for temperature in degrees C from datasheet
        print("GyX = ", GyX)
        print("GyY = ", GyY)
        print("GyZ = ", GyZ)
        sleep(1.0)

        b += 1
        print(b)
        if b == 2:
            print('La lecture des data du MPU-6050 a ete executee normalemment')
            connection.flush_input()
            ard.endTransmission(True)
            connection.close()
            return b
            exit()

#MPU_6050_read_data()