# Class to read data from the (GY-521) MPU6050 Accelerometer/Gyro Module
# Ported to MicroPython by Warayut Poomiwatracanont JAN 2023
# Original repo https://github.com/nickcoutsos/MPU-6050-Python
# and https://github.com/CoreElectronics/CE-PiicoDev-MPU6050-MicroPython-Module

from math import sqrt, atan2
from machine import Pin, SoftI2C
from time import sleep_ms
import MPU6050

error_msg = "\nError \n"
i2c_err_str = "ESP32 could not communicate with module at address 0x{:02X}, check wiring"




def signedIntFromBytes(x, endian="big"):
    y = int.from_bytes(x, endian)
    if (y >= 0x8000):
        return -((65535 - y) + 1)
    else:
        return y
    

# Empieza clase aqui




# Termina clase aqui


#from os import listdir, chdir
#from machine import Pin
#from time import sleep_ms

mpu = MPU6050()

# List all files directory.
#print("Root directory: {} \n".format(listdir()))

# Change filename and fileformat here.
#filename = "{}%s.{}".format("data_logs", "txt")

# Increment the filename number if the file already exists.
#i = 0
#while (filename % i) in listdir():
#    i += 1

# Save file in path /
#with open(filename % i, "w") as f:
cols = ["Temp", "AcX", "AcY", "AcZ"]
#    f.write(",".join(cols) + "\n")
    
while True:
    # Accelerometer Data
    accel = mpu.read_accel_data() # read the accelerometer [ms^-2]
    aX = accel["x"]
    aY = accel["y"]
    aZ = accel["z"]
    print("x: " + str(aX) + " y: " + str(aY) + " z: " + str(aZ))

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

    # Rough Temperature
    temp = mpu.read_temperature()   # read the device temperature [degC]
    print("Temperature: " + str(temp) + "°C")

    # G-Force
    # gforce = mpu.read_accel_abs(g=True) # read the absolute acceleration magnitude
    # print("G-Force: " + str(gforce))

    # Write to file
    data = {"Temp" : temp,
            "AcX" : aX,
            "AcY" : aY,
            "AcZ" : aZ
            }
    
    push = [  str(data[k]) for k in cols ]
    row = ",".join(push)
    #f.write(row + "\n")
    
    # Time Interval Delay in millisecond (ms)
    sleep_ms(100)