import mpu6050, buttons, display

def convAccel(value): # Converts 16-bit value to 2g range
    return round((value*2)/32768,4)

def convGyro(value): # Converts 16-bit value to 250 degree range
    return round((value*250)/32768,4)

def conv(value,size): # Converts to length of the line
    return value//(32768//size)

def graph(px, py, x, y, z):
    size = 50
    display.drawRect(px,py,size*2,size*2,False,0xFFFFFF)
    display.drawLine(px+size, py+size, px+size+conv(x,size), py+size, 0xFF0000)
    display.drawLine(px+size, py+size, px+size, py+size+conv(y,size), 0x00FF00)
    display.drawLine(px+size, py+size, px+size+conv(z,size), py+size+conv(z,size), 0x0000FF)

mpu6050.configure_accel_range(mpu6050.ACCEL_RANGE_2G) # 2, 4, 8, 16
mpu6050.configure_gyro_range(mpu6050.GYRO_RANGE_250) #250, 500, 1000, 2000

while True:
    accel = mpu6050.acceleration()
    gyro = mpu6050.gyroscope()
    display.drawFill(0x000000)
    ay = accel[0] # 16-bits signed value representing 0-2g
    ax = accel[1]
    az = accel[2]
    gy = gyro[0] # 16-bits signed value representing 0-250 degrees of change
    gx = gyro[1]
    gz = gyro[2]
    display.drawText(0,0,"Acceleration (g)\nX = "+str(convAccel(ax))+"\nY = "+str(convAccel(ay))+"\nZ = "+str(convAccel(az))+"\n",0xFFFFFF,"roboto_regular18")
    display.drawText(0,120,"Gyroscope (deg)\nX = "+str(convGyro(gx))+"\nY = "+str(convGyro(gy))+"\nZ = "+str(convGyro(gz))+"\n",0xFFFF00,"roboto_regular18")
    graph(210, 10,ax,ay,az)
    graph(210,120,gx,gy,gz)
    display.flush()