import bhi160
import display
import utime
import buttons


disp = display.open()
sensor = 0
sensors = [{"sensor": bhi160.BHI160Orientation(), "name": "Orientation"}]
mymode = "Plain"

def renderBlobPlain(sample):
    drifty=int(sample.y/1)
    if drifty<-25:
        drifty=-25
    if drifty>25:
        drifty=25
    driftz=int(sample.z/1)
    if driftz<-25:
        driftz=-25    
    if driftz>25:
        driftz=25
    if -2 < drifty < 2 and -2 < driftz < 2:
        colo=[0,200,0]
    else:
        colo=[255,0,0]
    disp.circ(80+driftz,40+drifty,5,col=colo,filled=True,size=2)

def renderBlobLevel(sample):
    driftz=int(sample.z/1)
    if driftz<-40:
        driftz=-40    
    if driftz>40:
        driftz=40
    if -2 < driftz < 2:
        colo=[0,200,0]
    else:
        colo=[255,0,0]
    disp.circ(80+driftz,40,5,col=colo,filled=True,size=2)


def mainPlain(sample):
    disp.rect(50,10,110,70,col=[200,200,200],filled=True,size=2)
    disp.circ(80,40,1,col=[0,0,0],filled=True,size=2)
    renderBlobPlain(sample)

def mainLevel(sample):
    disp.rect(0,35,159,45,col=[200,200,200],filled=True,size=2)
    disp.rect(78,25,82,55,col=[200,200,200],filled=True,size=2)
    renderBlobLevel(sample)

def main():
    mymode="Plain"
    while True:
        samples = sensors[sensor]["sensor"].read()
        if len(samples) > 0:
            disp.clear()
            sample = samples[0]
            print("X: "+str(sample.x) +" Y: "+str(sample.y)+" Z: "+str(sample.z))
            if mymode == "Plain":
                mainPlain(sample)
            if mymode == "Level":
                mainLevel(sample)
            disp.update()        
            pressed = buttons.read(buttons.BOTTOM_LEFT | buttons.BOTTOM_RIGHT)
            if pressed & buttons.BOTTOM_LEFT != 0:
                print("Level")
                mymode="Level"
            if pressed & buttons.BOTTOM_RIGHT != 0:
                print("Plain")
                mymode="Plain"
            utime.sleep(0.1)
main()