#import lis3dh, time, math
import lis3dh, math

#from machine import Pin, I2C
from machine import I2C


#i2c = I2C(sda=Pin(21), scl=Pin(22)) # Correct I2C pins for TinyPICO
i2c = I2C(53,52,400,2) #SimpleP2

imu = lis3dh.LIS3DH_I2C(i2c)

last_convert_time = 0
convert_interval = 100 #ms
pitch = 0
roll = 0

# Convert acceleration to Pitch and Roll
def convert_accell_rotation( vec ):
    x_Buff = vec[0] # x
    y_Buff = vec[1] # y
    z_Buff = vec[2] # z

    global last_convert_time, convert_interval, roll, pitch

    # We only want to re-process the values every 100 ms
    #if last_convert_time < time.ticks_ms():
    #    last_convert_time = time.ticks_ms() + convert_interval

    roll = math.atan2(y_Buff , z_Buff) * 57.3
    pitch = math.atan2((- x_Buff) , math.sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3

    # Return the current values in roll and pitch
    return ( roll, pitch )

# If we have found the LIS3DH
if imu.device_check():
    # Set range of accelerometer (can be RANGE_2_G, RANGE_4_G, RANGE_8_G or RANGE_16_G).
    imu.range = lis3dh.RANGE_2_G

    # Loop forever printing values
    while True:
        # Read accelerometer values (in m / s ^ 2).  Returns a 3-tuple of x, y,
        # z axis values.  Divide them by 9.806 to convert to Gs.
        x, y, z = [value / lis3dh.STANDARD_GRAVITY for value in imu.acceleration]
        print("x = %0.3f G, y = %0.3f G, z = %0.3f G" % (x, y, z))

        # Convert acceleration to Pitch and Roll and print values
        p, r = convert_accell_rotation( imu.acceleration )
        print("pitch = %0.2f, roll = %0.2f" % (p,r))

        # Small delay to keep things responsive but give time for interrupt processing.
        #time.sleep(0.1)